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
32using namespace upm;
33using namespace std;
34
35
36NUNCHUCK::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
63NUNCHUCK::~NUNCHUCK()
64{
65  mraa_i2c_stop(m_i2c);
66}
67
68bool 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
82int 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
93bool 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
107void 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