diff --git a/examples/nunchuck.rs b/examples/nunchuck.rs deleted file mode 100644 index 65bb731e..00000000 --- a/examples/nunchuck.rs +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2015, Paul Osborne -// -// Licensed under the Apache License, Version 2.0 or the MIT license -// , at your -// option. This file may not be copied, modified, or distributed -// except according to those terms. - -// Reads data from Wii Nunchuck - -extern crate i2cdev; -extern crate docopt; - -#[cfg(any(target_os = "linux", target_os = "android"))] -use i2cdev::linux::*; -#[cfg(any(target_os = "linux", target_os = "android"))] -use i2cdev::sensors::nunchuck::*; - -use std::env::args; -use docopt::Docopt; - -const USAGE: &'static str = " -Reading Wii Nunchuck data via Linux i2cdev. - -Usage: - nunchuck - nunchuck (-h | --help) - nunchuck --version - -Options: - -h --help Show this help text. - --version Show version. -"; - -#[cfg(not(any(target_os = "linux", target_os = "android")))] -fn main() {} - -#[cfg(any(target_os = "linux", target_os = "android"))] -fn main() { - let args = Docopt::new(USAGE) - .and_then(|d| d.argv(args().into_iter()).parse()) - .unwrap_or_else(|e| e.exit()); - let device = args.get_str(""); - let i2cdev = LinuxI2CDevice::new(device, NUNCHUCK_SLAVE_ADDR).unwrap(); - match Nunchuck::new(i2cdev) { - Err(err) => { - println!("Unable to open {:?}, {:?}", device, err); - } - Ok(mut nunchuck) => { - loop { - match nunchuck.read() { - Ok(reading) => println!("{:?}", reading), - Err(err) => println!("Error: {:?}", err), - }; - } - } - } -} diff --git a/examples/sensors.rs b/examples/sensors.rs deleted file mode 100644 index 62cb9702..00000000 --- a/examples/sensors.rs +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2015, Paul Osborne -// -// Licensed under the Apache License, Version 2.0 or the MIT license -// , at your -// option. This file may not be copied, modified, or distributed -// except according to those terms. - -// Reads data from several different sensors all on the same i2c bus -// - Analog Devices ADXL345 Digital Accelerometer -// - Freescale MPL115A2 Digital Barometer/Thermometer -// - Bosch BNO055 9-Axis O Absolute Orientation Sensor - -extern crate i2cdev; -extern crate docopt; - -use std::thread; -use std::time::Duration; -use std::env::args; -use docopt::Docopt; -use i2cdev::sensors::{Thermometer, Barometer, Accelerometer}; -use i2cdev::sensors::mpl115a2_barometer::*; -use i2cdev::sensors::adxl345_accelerometer::*; -#[cfg(any(target_os = "linux", target_os = "android"))] -use i2cdev::linux::*; - -const USAGE: &'static str = " -Reading sensor data from a variety of sensors - -Usage: - sensors - sensors (-h | --help) - sensors --version - -Options: - -h --help Show this help text. - --version Show version. -"; - -#[cfg(not(any(target_os = "linux", target_os = "android")))] -fn main() {} - -#[cfg(any(target_os = "linux", target_os = "android"))] -fn main() { - let args = Docopt::new(USAGE) - .and_then(|d| d.argv(args().into_iter()).parse()) - .unwrap_or_else(|e| e.exit()); - let device = args.get_str(""); - let mpl115a2_i2cdev = LinuxI2CDevice::new(device, MPL115A2_I2C_ADDR).unwrap(); - let adxl345_i2cdev = LinuxI2CDevice::new(device, 0x53).unwrap(); - - let mut mpl115a2 = MPL115A2BarometerThermometer::new(mpl115a2_i2cdev).unwrap(); - let mut adxl345 = ADXL345Accelerometer::new(adxl345_i2cdev).unwrap(); - - println!("== ADXL345 ID: 0x{:X} ==", adxl345.device_id().unwrap()); - - loop { - let accel = adxl345.accelerometer_sample().unwrap(); - println!("Temperature: {:?} C", - mpl115a2.temperature_celsius().unwrap()); - println!("Pressure: {:?} kPa", mpl115a2.pressure_kpa().unwrap()); - println!("Accel: {:?}", accel); - println!("Accel Tot: {:?}", - (accel.x.powi(2) + accel.y.powi(2) + accel.z.powi(2)).sqrt()); - thread::sleep(Duration::from_millis(1000)); - } -} diff --git a/src/lib.rs b/src/lib.rs index bcb47632..ff6f1f1f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -27,7 +27,6 @@ extern crate nix; mod ffi; pub mod core; -pub mod sensors; #[cfg(any(target_os = "linux", target_os = "android"))] pub mod linux; diff --git a/src/sensors/adxl345_accelerometer.rs b/src/sensors/adxl345_accelerometer.rs deleted file mode 100644 index c91b34ff..00000000 --- a/src/sensors/adxl345_accelerometer.rs +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2015, Paul Osborne -// -// Licensed under the Apache License, Version 2.0 or the MIT license -// , at your -// option. This file may not be copied, modified, or distributed -// except according to those terms. - -#![allow(dead_code)] // register map - -use sensors::{Accelerometer, AccelerometerSample}; -use core::I2CDevice; -use byteorder::{ByteOrder, LittleEndian}; - -// TODO: read/write data format (for now, assumed 0x00) - -pub const SLAVE_ADDR_PRIMARY: u16 = 0x1D; -pub const SLAVE_ADDR_ALT: u16 = 0x53; - -const REGISTER_DEVID: u8 = 0x00; -const REGSITER_THRESH_TAP: u8 = 0x1D; -const REGISTER_OFSX: u8 = 0x1E; -const REGISTER_OFSY: u8 = 0x1F; -const REGISTER_OFSZ: u8 = 0x20; -const REGISTER_DUR: u8 = 0x21; -const REGISTER_LATENT: u8 = 0x22; -const REGISTER_WINDOW: u8 = 0x23; -const REGISTER_THRESH_ACT: u8 = 0x24; -const REGISTER_THRESH_INACT: u8 = 0x25; -const REGISTER_TIME_INACT: u8 = 0x26; -const REGISTER_ACT_INACT_CTL: u8 = 0x27; -const REGISTER_THRESH_FF: u8 = 0x28; -const REGISTER_TIME_FF: u8 = 0x29; -const REGISTER_TAP_AXES: u8 = 0x2A; -const REGISTER_ACT_TAP_STATUS: u8 = 0x2B; -const REGISTER_BW_RATE: u8 = 0x2C; -const REGISTER_POWER_CTL: u8 = 0x2D; -const REGISTER_INT_ENABLE: u8 = 0x2E; -const REGISTER_INT_MAP: u8 = 0x2F; -const REGISTER_INT_SOURCE: u8 = 0x30; -const REGISTER_DATA_FORMAT: u8 = 0x31; -const REGISTER_X0: u8 = 0x32; -const REGISTER_X1: u8 = 0x33; -const REGISTER_Y0: u8 = 0x34; -const REGISTER_Y1: u8 = 0x35; -const REGISTER_Z0: u8 = 0x36; -const REGISTER_Z1: u8 = 0x37; -const REGISTER_FIFO_CTL: u8 = 0x38; -const REGISTER_FIFO_STATUS: u8 = 0x39; - -#[repr(u8)] -#[allow(non_camel_case_types)] -enum ADXL345DataRate { - RATE_3200HZ = 0x0F, - RATE_1600HZ = 0x0E, - RATE_800HZ = 0x0D, - RATE_400HZ = 0x0C, - RATE_200HZ = 0x0B, - RATE_100HZ = 0x0A, - RATE_50HZ = 0x09, - RATE_25HZ = 0x08, - RATE_12HZ5 = 0x07, - RATE_6HZ25 = 0x06, -} - -pub struct ADXL345Accelerometer { - i2cdev: T, -} - -impl ADXL345Accelerometer where T: I2CDevice + Sized -{ - /// Create a new accelerometer handle for the given path/addr - /// - /// The `SLAVE_ADDR_*` constants from this module should be - /// used to select either the primary or alternative slave - /// address (dependent on `ALT ADDRESS` pin) - pub fn new(mut i2cdev: T) -> Result, T::Error> { - // setup standy mode to configure - try!(i2cdev.smbus_write_byte_data(REGISTER_POWER_CTL, 0x00)); - - // configure some defaults - try!(i2cdev.smbus_write_byte_data(REGISTER_BW_RATE, ADXL345DataRate::RATE_1600HZ as u8)); - try!(i2cdev.smbus_write_byte_data(REGISTER_DATA_FORMAT, 0x08)); - try!(i2cdev.smbus_write_byte_data(REGISTER_OFSX, 0xFD)); - try!(i2cdev.smbus_write_byte_data(REGISTER_OFSY, 0x03)); - try!(i2cdev.smbus_write_byte_data(REGISTER_OFSZ, 0xFE)); - - // put device in measurement mode - try!(i2cdev.smbus_write_byte_data(REGISTER_POWER_CTL, 0x08)); - - Ok(ADXL345Accelerometer { i2cdev: i2cdev }) - } - - /// Get the device id - pub fn device_id(&mut self) -> Result { - self.i2cdev.smbus_read_byte_data(REGISTER_DEVID) - } -} - -const ACCEL_RANGE: f32 = 2.0; // +- 2G (with defaults) -const ACCEL_BITS: u8 = 10; // 10-bit resolution - -impl Accelerometer for ADXL345Accelerometer where T: I2CDevice + Sized -{ - type Error = T::Error; - - fn accelerometer_sample(&mut self) -> Result { - // datasheet recommends multi-byte read to avoid reading - // an inconsistent set of data - let mut buf: [u8; 6] = [0u8; 6]; - try!(self.i2cdev.write(&[REGISTER_X0])); - try!(self.i2cdev.read(&mut buf)); - - let x: i16 = LittleEndian::read_i16(&[buf[0], buf[1]]); - let y: i16 = LittleEndian::read_i16(&[buf[2], buf[3]]); - let z: i16 = LittleEndian::read_i16(&[buf[4], buf[5]]); - Ok(AccelerometerSample { - x: (x as f32 / 1023.0) * (ACCEL_RANGE * 2.0), - y: (y as f32 / 1023.0) * (ACCEL_RANGE * 2.0), - z: (z as f32 / 1023.0) * (ACCEL_RANGE * 2.0), - }) - } -} diff --git a/src/sensors/mod.rs b/src/sensors/mod.rs deleted file mode 100644 index 7abc2f4a..00000000 --- a/src/sensors/mod.rs +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2015, Paul Osborne -// -// Licensed under the Apache License, Version 2.0 or the MIT license -// , at your -// option. This file may not be copied, modified, or distributed -// except according to those terms. - -use std::error::Error; - -pub mod adxl345_accelerometer; -pub mod mpl115a2_barometer; -pub mod nunchuck; -// pub mod bno055_orientation; - -#[derive(Debug)] -pub struct AccelerometerSample { - /// x-axis G's - pub x: f32, - /// y-axis G's - pub y: f32, - /// z-axis G's - pub z: f32, -} - -/// Trait for sensors that provide access to accelerometer readings (3-axis) -pub trait Accelerometer { - type Error: Error; - - /// Grab an accelerometer sample from the device - fn accelerometer_sample(&mut self) -> Result; -} - -/// Trait for sensors that provide access to temperature readings -pub trait Thermometer { - type Error: Error; - - /// Get a temperature from the sensor in degrees celsius - /// - /// Returns Some(temperature) if available, otherwise returns - /// None - fn temperature_celsius(&mut self) -> Result; -} - -/// Trait for sensors that provide access to pressure readings -pub trait Barometer { - type Error: Error; - - /// Get a pressure reading from the sensor in kPa - /// - /// Returns Some(temperature) if avialable, otherwise returns - /// None - fn pressure_kpa(&mut self) -> Result; -} diff --git a/src/sensors/mpl115a2_barometer.rs b/src/sensors/mpl115a2_barometer.rs deleted file mode 100644 index 316ada5e..00000000 --- a/src/sensors/mpl115a2_barometer.rs +++ /dev/null @@ -1,229 +0,0 @@ -// Copyright 2015, Paul Osborne -// -// Licensed under the Apache License, Version 2.0 or the MIT license -// , at your -// option. This file may not be copied, modified, or distributed -// except according to those terms. - -#![allow(dead_code)] - -use sensors::{Thermometer, Barometer}; -use std::thread; -use std::time::Duration; -use std::error::Error; -use core::I2CDevice; -use byteorder::{ByteOrder, BigEndian}; - -pub const MPL115A2_I2C_ADDR: u16 = 0x60; // appears to always be this - -const REGISTER_ADDR_PADC: u8 = 0x00; -const REGISTER_ADDR_TADC: u8 = 0x02; -const REGISTER_ADDR_A0: u8 = 0x04; // other coefficients follow -const REGISTER_ADDR_START_CONVERSION: u8 = 0x12; - -/// Provides access to the MPL115A2 Temperature and Pressure Sensor -/// -/// http://cache.freescale.com/files/sensors/doc/data_sheet/MPL115A2.pdf -pub struct MPL115A2BarometerThermometer { - pub i2cdev: T, - pub coeff: MPL115A2Coefficients, -} - -/// In order to get either the temperature or humdity it is -/// necessary to read several different values from the chip. -/// -/// These are not generally useful in and of themselves but -/// are used for calculating temperature/pressure. The structure -/// is exposed externally as they *could* be useful for some -/// unknown use case. Generally, you shouldn't ever need -/// to use this directly. -/// -/// One use case for use of this struct directly would be for -/// getting both temperature and pressure in a single call. -#[derive(Debug)] -pub struct MPL115A2RawReading { - padc: u16, // 10-bit pressure ADC output value - tadc: u16, // 10-bit pressure ADC output value -} - -/// The sensors has several coefficients that must be used in order -/// to calculate a correct value for pressure/temperature. -/// -/// This structure provides access to those. It is usually only -/// necessary to read these coefficients once per interaction -/// with the acclerometer. It does not need to be read again -/// on each sample. -#[derive(Debug)] -pub struct MPL115A2Coefficients { - a0: f32, // 16 bits, 1 sign, 12 int, 3 fractional, 0 dec pt 0 pad - b1: f32, // 16 bits, 1 sign, 2 int, 13 fractional, 0 dec pt 0 pad - b2: f32, // 16 bits, 1 sign, 1 int, 14 fractional, 0 dec pt 0 pad - c12: f32, // 16 bits, 1 sign, 0 int, 13 fractional, 9 dec pt 0 pad -} - -fn calc_coefficient(msb: u8, - lsb: u8, - integer_bits: i32, - fractional_bits: i32, - dec_pt_zero_pad: i32) - -> f32 { - // If values are less than 16 bytes, need to adjust - let extrabits = 16 - integer_bits - fractional_bits - 1; - let rawval: i16 = BigEndian::read_i16(&[msb, lsb]); - let adj = (rawval as f32 / 2_f32.powi(fractional_bits + extrabits)) / - 10_f32.powi(dec_pt_zero_pad); - adj -} - -impl MPL115A2Coefficients { - /// Convert a slice of data values of length 8 to coefficients - /// - /// This should be built from a read of registers 0x04-0x0B in - /// order. This gets the raw, unconverted value of each - /// coefficient. - pub fn new(i2cdev: &mut I2CDevice) -> Result { - let mut buf: [u8; 8] = [0; 8]; - try!(i2cdev.write(&[REGISTER_ADDR_A0])); - try!(i2cdev.read(&mut buf)); - Ok(MPL115A2Coefficients { - a0: calc_coefficient(buf[0], buf[1], 12, 3, 0), - b1: calc_coefficient(buf[2], buf[3], 2, 13, 0), - b2: calc_coefficient(buf[4], buf[5], 1, 14, 0), - c12: calc_coefficient(buf[6], buf[7], 0, 13, 9), - }) - } -} - - -impl MPL115A2RawReading { - /// Create a new reading from the provided I2C Device - pub fn new(i2cdev: &mut I2CDevice) -> Result { - // tell the chip to do an ADC read so we can get updated values - try!(i2cdev.smbus_write_byte_data(REGISTER_ADDR_START_CONVERSION, 0x00)); - - // maximum conversion time is 3ms - thread::sleep(Duration::from_millis(3)); - - // The SMBus functions read word values as little endian but that is not - // what we want - let mut buf = [0_u8; 4]; - try!(i2cdev.write(&[REGISTER_ADDR_PADC])); - try!(i2cdev.read(&mut buf)); - let padc: u16 = BigEndian::read_u16(&buf) >> 6; - let tadc: u16 = BigEndian::read_u16(&buf[2..]) >> 6; - Ok(MPL115A2RawReading { - padc: padc, - tadc: tadc, - }) - } - - /// Calculate the temperature in centrigrade for this reading - pub fn temperature_celsius(&self) -> f32 { - (self.tadc as f32 - 498.0) / -5.35 + 25.0 - } - - /// Calculate the pressure in pascals for this reading - pub fn pressure_kpa(&self, coeff: &MPL115A2Coefficients) -> f32 { - // Pcomp = a0 + (b1 + c12 * Tadc) * Padc + b2 * Tadc - // Pkpa = Pcomp * ((115 - 50) / 1023) + 50 - let pcomp: f32 = coeff.a0 + (coeff.b1 + coeff.c12 * self.tadc as f32) * self.padc as f32 + - (coeff.b2 * self.tadc as f32); - - // scale has 1023 bits of range from 50 kPa to 115 kPa - let pkpa: f32 = pcomp * ((115.0 - 50.0) / 1023.0) + 50.0; - pkpa - } -} - - -impl MPL115A2BarometerThermometer where T: I2CDevice + Sized -{ - /// Create sensor accessor for MPL115A2 on the provided i2c bus path - pub fn new(mut i2cdev: T) -> Result, T::Error> { - let coeff = try!(MPL115A2Coefficients::new(&mut i2cdev)); - Ok(MPL115A2BarometerThermometer { - i2cdev: i2cdev, - coeff: coeff, - }) - } -} - -impl Barometer for MPL115A2BarometerThermometer where T: I2CDevice + Sized -{ - type Error = T::Error; - - fn pressure_kpa(&mut self) -> Result { - let reading = try!(MPL115A2RawReading::new(&mut self.i2cdev)); - Ok(reading.pressure_kpa(&self.coeff)) - } -} - -impl Thermometer for MPL115A2BarometerThermometer where T: I2CDevice + Sized -{ - type Error = T::Error; - - fn temperature_celsius(&mut self) -> Result { - let reading = try!(MPL115A2RawReading::new(&mut self.i2cdev)); - Ok(reading.temperature_celsius()) - } -} - - -#[cfg(test)] -mod tests { - use super::*; - use super::calc_coefficient; - use sensors::*; - use mock::MockI2CDevice; - - macro_rules! assert_almost_eq { - ($left:expr, $right:expr) => ({ - match (&($left), &($right)) { - (left_val, right_val) => { - if (*left_val - *right_val).abs() > 0.0001 { - panic!("assertion failed: ({:?} != {:?})", *left_val, *right_val); - } - } - } - }) - } - - fn make_dev(mut i2cdev: MockI2CDevice) -> MPL115A2BarometerThermometer { - (&mut i2cdev.regmap).write_regs(0x04, - &[74, 98 /* A0 */, 165, 150 /* B1 */, 182, - 106 /* B2 */, 63, 232]); // C12 - MPL115A2BarometerThermometer::new(i2cdev).unwrap() - } - - #[test] - fn test_calc_coefficient() { - // unsigned simple - assert_almost_eq!(calc_coefficient(0x00, 0b1000, 12, 3, 0), 1.0); - // signed simple - assert_almost_eq!(calc_coefficient(0xFF, 0xF8, 12, 3, 0), -1.0); - // pure integer (negative) - assert_almost_eq!(calc_coefficient(0x80, 0x00, 15, 0, 0), -32_768_f32); - // no integer part, zero padding, negative - assert_almost_eq!(calc_coefficient(0x00, 0x01, 15, 0, 10), 0.000_000_000_1); - } - - #[test] - fn test_basic_pressure_read() { - let mut i2cdev = MockI2CDevice::new(); - (&mut i2cdev.regmap).write_regs(0x00, &[0x6e, 0xc0, 0x81, 0x40]); - - let mut dev = make_dev(i2cdev); - assert_almost_eq!(dev.pressure_kpa().unwrap(), 83.93877); - } - - #[test] - fn test_basic_temp_read() { - let mut i2cdev = MockI2CDevice::new(); - (&mut i2cdev.regmap).write_regs(0, &[0x6e, 0xc0, 0x81, 0x40]); - - let mut dev = make_dev(i2cdev); - assert_almost_eq!(dev.temperature_celsius().unwrap(), 21.448599); - } - -} diff --git a/src/sensors/nunchuck.rs b/src/sensors/nunchuck.rs deleted file mode 100644 index 4a196a97..00000000 --- a/src/sensors/nunchuck.rs +++ /dev/null @@ -1,175 +0,0 @@ -// Copyright 2015, Paul Osborne -// -// Licensed under the Apache License, Version 2.0 or the MIT license -// , at your -// option. This file may not be copied, modified, or distributed -// except according to those terms. - -// Reads data from Wii Nunchuck - -use std::error::Error; -use std::thread; -use std::time::Duration; -use std::fmt; - -use core::I2CDevice; - -pub const NUNCHUCK_SLAVE_ADDR: u16 = 0x52; - -#[derive(Debug)] -pub enum NunchuckError { - Error(E), - ParseError, -} - -impl fmt::Display for NunchuckError { - fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - match *self { - NunchuckError::Error(ref e) => fmt::Display::fmt(e, f), - NunchuckError::ParseError => fmt::Display::fmt(self.description(), f), - } - } -} - -impl Error for NunchuckError { - fn description(&self) -> &str { - match *self { - NunchuckError::Error(ref e) => e.description(), - NunchuckError::ParseError => "Unable to Parse Data", - } - } - - fn cause(&self) -> Option<&Error> { - match *self { - NunchuckError::Error(ref e) => Some(e), - NunchuckError::ParseError => None, - } - } -} - -// TODO: Move Nunchuck code out to be an actual sensor and add tests - -#[derive(Debug)] -pub struct NunchuckReading { - joystick_x: u8, - joystick_y: u8, - accel_x: u16, // 10-bit - accel_y: u16, // 10-bit - accel_z: u16, // 10-bit - c_button_pressed: bool, - z_button_pressed: bool, -} - -impl NunchuckReading { - pub fn from_data(data: &[u8]) -> Option { - if data.len() < 6 { - None - } else { - Some(NunchuckReading { - joystick_x: data[0], - joystick_y: data[1], - accel_x: (data[2] as u16) << 2 | ((data[5] as u16 >> 6) & 0b11), - accel_y: (data[3] as u16) << 2 | ((data[5] as u16 >> 4) & 0b11), - accel_z: (data[4] as u16) << 2 | ((data[5] as u16 >> 2) & 0b11), - c_button_pressed: (data[5] & 0b10) == 0, - z_button_pressed: (data[5] & 0b01) == 0, - }) - } - } -} - -pub struct Nunchuck { - i2cdev: T, -} - -impl Nunchuck where T: I2CDevice -{ - /// Create a new Wii Nunchuck - /// - /// This method will open the provide i2c device file and will - /// send the required init sequence in order to read data in - /// the future. - pub fn new(i2cdev: T) -> Result, T::Error> { - let mut nunchuck = Nunchuck { i2cdev: i2cdev }; - try!(nunchuck.init()); - Ok(nunchuck) - } - - #[cfg(test)] - pub fn get_i2cdev(&mut self) -> &mut T { - &mut self.i2cdev - } - - /// Send the init sequence to the Wii Nunchuck - pub fn init(&mut self) -> Result<(), T::Error> { - // These registers must be written; the documentation is a bit - // lacking but it appears this is some kind of handshake to - // perform unencrypted data tranfers - try!(self.i2cdev.smbus_write_byte_data(0xF0, 0x55)); - try!(self.i2cdev.smbus_write_byte_data(0xFB, 0x00)); - Ok(()) - } - - pub fn read(&mut self) -> Result> { - let mut buf: [u8; 6] = [0; 6]; - - // tell the nunchuck to prepare a sample - try!(self.i2cdev.smbus_write_byte(0x00).map_err(NunchuckError::Error)); - - // now, read it! - thread::sleep(Duration::from_millis(10)); - try!(self.i2cdev.read(&mut buf).map_err(NunchuckError::Error)); - NunchuckReading::from_data(&buf).ok_or(NunchuckError::ParseError) - } -} - - -#[cfg(test)] -mod test { - use super::*; - use core::I2CDevice; - use mock::MockI2CDevice; - - #[test] - fn test_intialization() { - // write out some "bad" values to start out with so we know the - // write happens - let mut i2cdev = MockI2CDevice::new(); - i2cdev.smbus_write_byte_data(0xF0, 0xFF).unwrap(); - i2cdev.smbus_write_byte_data(0xFB, 0xFF).unwrap(); - - // these values must be written out for things to work - let mut dev = Nunchuck::new(i2cdev).unwrap(); - assert_eq!(dev.get_i2cdev().smbus_read_byte_data(0xF0).unwrap(), 0x55); - assert_eq!(dev.get_i2cdev().smbus_read_byte_data(0xFB).unwrap(), 0x00); - } - - #[test] - fn test_read_zeroed_out() { - let mut dev = Nunchuck::new(MockI2CDevice::new()).unwrap(); - let reading = dev.read().unwrap(); - assert_eq!(reading.joystick_x, 0); - assert_eq!(reading.joystick_y, 0); - assert_eq!(reading.accel_x, 0); - assert_eq!(reading.accel_y, 0); - assert_eq!(reading.accel_z, 0); - assert_eq!(reading.c_button_pressed, true); - assert_eq!(reading.z_button_pressed, true); - } - - #[test] - fn test_read_sample_data() { - let mut i2cdev = MockI2CDevice::new(); - i2cdev.write(&[0, 127, 128, 191, 129, 144, 71]).unwrap(); - let mut dev = Nunchuck::new(i2cdev).unwrap(); - let reading = dev.read().unwrap(); - assert_eq!(reading.joystick_x, 127); - assert_eq!(reading.joystick_y, 128); - assert_eq!(reading.accel_x, 765); - assert_eq!(reading.accel_y, 516); - assert_eq!(reading.accel_z, 577); - assert_eq!(reading.c_button_pressed, false); - assert_eq!(reading.z_button_pressed, false); - } -}