1 /*
2 * Author: Jon Trulson <jtrulson@ics.com>
3 * Copyright (c) 2015 Intel Corporation.
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining
6 * a copy of this software and associated documentation files (the
7 * "Software"), to deal in the Software without restriction, including
8 * without limitation the rights to use, copy, modify, merge, publish,
9 * distribute, sublicense, and/or sell copies of the Software, and to
10 * permit persons to whom the Software is furnished to do so, subject to
11 * the following conditions:
12 *
13 * The above copyright notice and this permission notice shall be
14 * included in all copies or substantial portions of the Software.
15 *
16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
20 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
21 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
22 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
23 */
24
25 #include <iostream>
26 #include <string>
27 #include <stdexcept>
28 #include <unistd.h>
29
30 #include "nunchuck.h"
31
32 using namespace upm;
33 using namespace std;
34
35
NUNCHUCK(int bus,uint8_t addr)36 NUNCHUCK::NUNCHUCK(int bus, uint8_t addr)
37 {
38 stickX = 0;
39 stickY = 0;
40 accelX = 0;
41 accelY = 0;
42 accelZ = 0;
43 buttonC = false;
44 buttonZ = false;
45
46 // setup our i2c link
47 if ( !(m_i2c = mraa_i2c_init(bus)) )
48 {
49 throw std::invalid_argument(std::string(__FUNCTION__) +
50 ": mraa_i2c_init() failed");
51 return;
52 }
53
54 mraa_result_t rv;
55
56 if ( (rv = mraa_i2c_address(m_i2c, addr)) != MRAA_SUCCESS )
57 {
58 throw std::invalid_argument(std::string(__FUNCTION__) +
59 ": mraa_i2c_address() failed");
60 }
61 }
62
~NUNCHUCK()63 NUNCHUCK::~NUNCHUCK()
64 {
65 mraa_i2c_stop(m_i2c);
66 }
67
writeByte(uint8_t reg,uint8_t byte)68 bool NUNCHUCK::writeByte(uint8_t reg, uint8_t byte)
69 {
70 mraa_result_t rv;
71
72 if ( (rv = mraa_i2c_write_byte_data(m_i2c, byte, reg)) != MRAA_SUCCESS )
73 {
74 throw std::runtime_error(std::string(__FUNCTION__) +
75 ": mraa_i2c_write_byte_data() failed");
76 return false;
77 }
78
79 return true;
80 }
81
readBytes(uint8_t reg,uint8_t * buffer,int len)82 int NUNCHUCK::readBytes(uint8_t reg, uint8_t *buffer, int len)
83 {
84 if (!len || !buffer)
85 return 0;
86
87 mraa_i2c_address(m_i2c, NUNCHUCK_I2C_ADDR);
88 mraa_i2c_write_byte(m_i2c, reg);
89
90 return mraa_i2c_read(m_i2c, buffer, len);
91 }
92
init()93 bool NUNCHUCK::init()
94 {
95 usleep(1000000);
96
97 // disable encryption
98 if (!writeByte(0xf0, 0x55))
99 return false;
100
101 if (!writeByte(0xfb, 0x00))
102 return false;
103
104 return true;
105 }
106
update()107 void NUNCHUCK::update()
108 {
109 const int bufsize = 6;
110 uint8_t buf[bufsize];
111 int rv;
112
113 rv = readBytes(0x00, buf, bufsize);
114
115 if (rv != bufsize)
116 {
117 throw std::runtime_error(std::string(__FUNCTION__) +
118 ": readBytes() failed");
119 return;
120 }
121
122 // analog stick X
123 stickX = buf[0];
124
125 // analog stick Y
126 stickY = buf[1];
127
128 // accelerometer X
129 accelX = ( (buf[2] << 2) | ((buf[5] & 0x0c) >> 2) );
130
131 // accelerometer Y
132 accelY = ( (buf[3] << 2) | ((buf[5] & 0x30) >> 4) );
133
134 // accelerometer Z
135 accelZ = ( (buf[4] << 2) | ((buf[5] & 0xc0) >> 6) );
136
137 // buttonC
138 if (buf[5] & 0x02)
139 buttonC = false;
140 else
141 buttonC = true;
142
143 // buttonZ
144 if (buf[5] & 0x01)
145 buttonZ = false;
146 else
147 buttonZ = true;
148 }
149
150