1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144

// Copyright 2015, Paul Osborne <osbpau@gmail.com>
//
// Licensed under the Apache License, Version 2.0 <LICENSE-APACHE or
// http://www.apache.org/license/LICENSE-2.0> or the MIT license
// <LICENSE-MIT or http://opensource.org/licenses/MIT>, at your
// option.  This file may not be copied, modified, or distributed
// except according to those terms.

// Reads data from Wii Nunchuck

use std::io::prelude::*;
use std::thread;

use core::{I2CDevice, I2CResult, I2CError};

pub const NUNCHUCK_SLAVE_ADDR: u16 = 0x52;

// 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<NunchuckReading> {
        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<T: I2CDevice> {
    i2cdev: T,
}

impl<T> Nunchuck<T> 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) -> I2CResult<Nunchuck<T>> {
        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) -> I2CResult<()> {
        // 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) -> I2CResult<NunchuckReading> {
        let mut buf: [u8; 6] = [0; 6];

        // tell the nunchuck to prepare a sample
        try!(self.i2cdev.smbus_write_byte(0x00));

        // now, read it!
        thread::sleep_ms(10);
        try!(self.i2cdev.read(&mut buf));
        match NunchuckReading::from_data(&buf) {
            Some(reading) => Ok(reading),
            None => Err(I2CError::Other("Unable to Parse Data"))
        }
    }
}


#[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.regmap.smbus_write_byte_data(0xF0, 0xFF).unwrap();
        i2cdev.regmap.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().regmap.smbus_read_byte_data(0xF0).unwrap(), 0x55);
        assert_eq!(dev.get_i2cdev().regmap.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.regmap.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);
    }
}