USB Host Shield 2.0
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
Wii.cpp
Go to the documentation of this file.
1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
2 
3  This software may be distributed and modified under the terms of the GNU
4  General Public License version 2 (GPL2) as published by the Free Software
5  Foundation and appearing in the file GPL2.TXT included in the packaging of
6  this file. Please note that GPL2 Section 2[b] requires that all works based
7  on this software must also be made publicly available under the terms of
8  the GPL2 ("Copyleft").
9 
10  Contact information
11  -------------------
12 
13  Kristian Lauszus, TKJ Electronics
14  Web : http://www.tkjelectronics.com
15  e-mail : kristianl@tkjelectronics.com
16 
17  IR camera support added by:
18  Allan Glover
19  adglover9.81@gmail.com
20  */
21 
22 #include "Wii.h"
23 #define DEBUG // Uncomment to print data for debugging
24 //#define EXTRADEBUG // Uncomment to get even more debugging data
25 //#define PRINTREPORT // Uncomment to print the report send by the Wii controllers
26 
27 const uint8_t LEDS[] PROGMEM = {
28  0x10, // LED1
29  0x20, // LED2
30  0x40, // LED3
31  0x80, // LED4
32 
33  0x90, // LED5
34  0xA0, // LED6
35  0xC0, // LED7
36  0xD0, // LED8
37  0xE0, // LED9
38  0xF0 // LED10
39 };
40 
41 const uint32_t BUTTONS[] PROGMEM = {
42  0x00008, // UP
43  0x00002, // RIGHT
44  0x00004, // DOWN
45  0x00001, // LEFT
46 
47  0, // Skip
48  0x00010, // PLUS
49  0x00100, // TWO
50  0x00200, // ONE
51 
52  0x01000, // MINUS
53  0x08000, // HOME
54  0x10000, // Z
55  0x20000, // C
56 
57  0x00400, // B
58  0x00800 // A
59 };
60 const uint32_t PROCONTROLLERBUTTONS[] PROGMEM = {
61  0x00100, // UP
62  0x00080, // RIGHT
63  0x00040, // DOWN
64  0x00200, // LEFT
65 
66  0, // Skip
67  0x00004, // PLUS
68  0x20000, // L3
69  0x10000, // R3
70 
71  0x00010, // MINUS
72  0x00008, // HOME
73  0, 0, // Skip
74 
75  0x04000, // B
76  0x01000, // A
77  0x00800, // X
78  0x02000, // Y
79 
80  0x00020, // L
81  0x00002, // R
82  0x08000, // ZL
83  0x00400 // ZR
84 };
85 
86 WII::WII(BTD *p, bool pair) :
87 pBtd(p) // pointer to USB class instance - mandatory
88 {
89  if (pBtd)
90  pBtd->registerServiceClass(this); // Register it as a Bluetooth service
91 
92  pBtd->pairWithWii = pair;
93 
94  HIDBuffer[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
95 
96  /* Set device cid for the control and intterrupt channelse - LSB */
97  control_dcid[0] = 0x60; //0x0060
98  control_dcid[1] = 0x00;
99  interrupt_dcid[0] = 0x61; //0x0061
100  interrupt_dcid[1] = 0x00;
101 
102  Reset();
103 }
104 
105 void WII::Reset() {
106  wiimoteConnected = false;
107  nunchuckConnected = false;
108  motionPlusConnected = false;
109  activateNunchuck = false;
110  motionValuesReset = false;
111  activeConnection = false;
112  pBtd->motionPlusInside = false;
113  pBtd->wiiUProController = false;
114  wiiUProControllerConnected = false;
115  l2cap_event_flag = 0; // Reset flags
116  l2cap_state = L2CAP_WAIT;
117 }
118 
119 void WII::disconnect() { // Use this void to disconnect any of the controllers
120  if (motionPlusConnected && !pBtd->motionPlusInside) { // Disable the Motion Plus extension
121 #ifdef DEBUG
122  Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80);
123 #endif
124  initExtension1(); // This will disable the Motion Plus extension
125  }
126  //First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection
127  pBtd->l2cap_disconnection_request(hci_handle, 0x0A, interrupt_scid, interrupt_dcid);
128  Reset();
129  l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
130 }
131 
132 void WII::ACLData(uint8_t* l2capinbuf) {
133  if (!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) {
134  if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
135  if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
136  pBtd->incomingWii = false;
137  pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
138  activeConnection = true;
139  hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
140  l2cap_state = L2CAP_WAIT;
141  }
142  }
143  }
144  if ((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000)) { // acl_handle_ok or it's a new connection
145  if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U
146  if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
147 #ifdef DEBUG
148  Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
149  PrintHex<uint8_t > (l2capinbuf[13], 0x80);
150  Notify(PSTR(" "), 0x80);
151  PrintHex<uint8_t > (l2capinbuf[12], 0x80);
152  Notify(PSTR(" "), 0x80);
153  PrintHex<uint8_t > (l2capinbuf[17], 0x80);
154  Notify(PSTR(" "), 0x80);
155  PrintHex<uint8_t > (l2capinbuf[16], 0x80);
156  Notify(PSTR(" "), 0x80);
157  PrintHex<uint8_t > (l2capinbuf[15], 0x80);
158  Notify(PSTR(" "), 0x80);
159  PrintHex<uint8_t > (l2capinbuf[14], 0x80);
160 #endif
161  } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) {
162  if (((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success
163  if (l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) { // Success
164  //Notify(PSTR("\r\nHID Control Connection Complete"), 0x80);
165  identifier = l2capinbuf[9];
166  control_scid[0] = l2capinbuf[12];
167  control_scid[1] = l2capinbuf[13];
168  l2cap_event_flag |= L2CAP_FLAG_CONTROL_CONNECTED;
169  } else if (l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) {
170  //Notify(PSTR("\r\nHID Interrupt Connection Complete"), 0x80);
171  identifier = l2capinbuf[9];
172  interrupt_scid[0] = l2capinbuf[12];
173  interrupt_scid[1] = l2capinbuf[13];
174  l2cap_event_flag |= L2CAP_FLAG_INTERRUPT_CONNECTED;
175  }
176  }
177  } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
178 #ifdef EXTRADEBUG
179  Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
180  PrintHex<uint8_t > (l2capinbuf[13], 0x80);
181  Notify(PSTR(" "), 0x80);
182  PrintHex<uint8_t > (l2capinbuf[12], 0x80);
183  Notify(PSTR(" SCID: "), 0x80);
184  PrintHex<uint8_t > (l2capinbuf[15], 0x80);
185  Notify(PSTR(" "), 0x80);
186  PrintHex<uint8_t > (l2capinbuf[14], 0x80);
187  Notify(PSTR(" Identifier: "), 0x80);
188  PrintHex<uint8_t > (l2capinbuf[9], 0x80);
189 #endif
190  if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
191  identifier = l2capinbuf[9];
192  control_scid[0] = l2capinbuf[14];
193  control_scid[1] = l2capinbuf[15];
194  l2cap_event_flag |= L2CAP_FLAG_CONNECTION_CONTROL_REQUEST;
195  } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
196  identifier = l2capinbuf[9];
197  interrupt_scid[0] = l2capinbuf[14];
198  interrupt_scid[1] = l2capinbuf[15];
199  l2cap_event_flag |= L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST;
200  }
201  } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
202  if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
203  if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
204  //Notify(PSTR("\r\nHID Control Configuration Complete"), 0x80);
205  identifier = l2capinbuf[9];
206  l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_SUCCESS;
207  } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
208  //Notify(PSTR("\r\nHID Interrupt Configuration Complete"), 0x80);
209  identifier = l2capinbuf[9];
210  l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS;
211  }
212  }
213  } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
214  if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
215  //Notify(PSTR("\r\nHID Control Configuration Request"), 0x80);
216  pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid);
217  } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
218  //Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80);
219  pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid);
220  }
221  } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
222  if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
223 #ifdef DEBUG
224  Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
225 #endif
226  identifier = l2capinbuf[9];
227  pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid);
228  Reset();
229  } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
230 #ifdef DEBUG
231  Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80);
232 #endif
233  identifier = l2capinbuf[9];
234  pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid);
235  Reset();
236  }
237  } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
238  if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
239  //Notify(PSTR("\r\nDisconnect Response: Control Channel"), 0x80);
240  identifier = l2capinbuf[9];
241  l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE;
242  } else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
243  //Notify(PSTR("\r\nDisconnect Response: Interrupt Channel"), 0x80);
244  identifier = l2capinbuf[9];
245  l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE;
246  }
247  }
248 #ifdef EXTRADEBUG
249  else {
250  identifier = l2capinbuf[9];
251  Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
252  PrintHex<uint8_t > (l2capinbuf[8], 0x80);
253  }
254 #endif
255  } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
256  //Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
257  if (wiimoteConnected) {
258  if (l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
259  if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
260  if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
261  ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
262  else if (wiiUProControllerConnected)
263  ButtonState = (uint32_t)(((~l2capinbuf[23]) & 0xFE) | ((uint16_t)(~l2capinbuf[24]) << 8) | ((uint32_t)((~l2capinbuf[25]) & 0x03) << 16));
264  else if (motionPlusConnected) {
265  if (l2capinbuf[20] & 0x02) // Only update the wiimote buttons, since the extension bytes are from the Motion Plus
266  ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000)));
267  else if (nunchuckConnected) // Update if it's a report from the Nunchuck
268  ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x0C) << 14));
269  //else if(classicControllerConnected) // Update if it's a report from the Classic Controller
270  } else if (nunchuckConnected) // The Nunchuck is directly connected
271  ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x03) << 16));
272  //else if(classicControllerConnected) // The Classic Controller is directly connected
273  else if (!unknownExtensionConnected)
274  ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
275 #ifdef PRINTREPORT
276  Notify(PSTR("ButtonState: "), 0x80);
277  PrintHex<uint32_t > (ButtonState, 0x80);
278  Notify(PSTR("\r\n"), 0x80);
279 #endif
280  if (ButtonState != OldButtonState) {
281  ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
282  OldButtonState = ButtonState;
283  }
284  }
285  if (l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer
286  accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5)) - 500;
287  accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500;
288  accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500;
289  wiimotePitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
290  wiimoteRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
291  }
292  switch (l2capinbuf[9]) {
293  case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV
294  wiiState = l2capinbuf[12]; // (0x01: Battery is nearly empty), (0x02: An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4)
295  batteryLevel = l2capinbuf[15]; // Update battery level
296  if (l2capinbuf[12] & 0x01) {
297 #ifdef DEBUG
298  Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80);
299 #endif
300  }
301  if (l2capinbuf[12] & 0x02) { // Check if a extension is connected
302 #ifdef DEBUG
303  if (!unknownExtensionConnected)
304  Notify(PSTR("\r\nExtension connected"), 0x80);
305 #endif
306  unknownExtensionConnected = true;
307 #ifdef WIICAMERA
308  if (!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
309 #endif
310  setReportMode(false, 0x35); // Also read the extension
311  } else {
312 #ifdef DEBUG
313  Notify(PSTR("\r\nExtension disconnected"), 0x80);
314 #endif
315  if (motionPlusConnected) {
316 #ifdef DEBUG
317  Notify(PSTR(" - from Motion Plus"), 0x80);
318 #endif
319  l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
320  if (!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false
321  nunchuckConnected = false;
322  //else if(classicControllerConnected)
323  } else if (nunchuckConnected) {
324 #ifdef DEBUG
325  Notify(PSTR(" - Nunchuck"), 0x80);
326 #endif
327  nunchuckConnected = false; // It must be the Nunchuck controller then
328  l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
329  setLedStatus();
330  setReportMode(false, 0x31); // If there is no extension connected we will read the button and accelerometer
331  } else {
332  setReportMode(false, 0x31); // If there is no extension connected we will read the button and accelerometer
333  }
334  }
335  break;
336  case 0x21: // Read Memory Data
337  if ((l2capinbuf[12] & 0x0F) == 0) { // No error
338  // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
339  if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
340 #ifdef DEBUG
341  Notify(PSTR("\r\nNunchuck connected"), 0x80);
342 #endif
343  l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED;
344  } else if (l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) {
345 #ifdef DEBUG
346  Notify(PSTR("\r\nMotion Plus connected"), 0x80);
347 #endif
348  l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED;
349  } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) {
350 #ifdef DEBUG
351  Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80);
352 #endif
353  motionPlusConnected = true;
354  } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
355 #ifdef DEBUG
356  Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80);
357 #endif
358  activateNunchuck = false;
359  motionPlusConnected = true;
360  nunchuckConnected = true;
361  } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) {
362 #ifdef DEBUG
363  Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80);
364  Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80);
365 #endif
366  stateCounter = 300; // Skip the rest in "L2CAP_CHECK_MOTION_PLUS_STATE"
367  } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) {
368 #ifdef DEBUG
369  Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80);
370 #endif
371  wiiUProControllerConnected = true;
372  }
373 #ifdef DEBUG
374  else {
375  Notify(PSTR("\r\nUnknown Device: "), 0x80);
376  PrintHex<uint8_t > (l2capinbuf[13], 0x80);
377  PrintHex<uint8_t > (l2capinbuf[14], 0x80);
378  Notify(PSTR("\r\nData: "), 0x80);
379  for (uint8_t i = 0; i < ((l2capinbuf[12] >> 4) + 1); i++) { // bit 4-7 is the length-1
380  PrintHex<uint8_t > (l2capinbuf[15 + i], 0x80);
381  Notify(PSTR(" "), 0x80);
382  }
383  }
384 #endif
385  }
386 #ifdef EXTRADEBUG
387  else {
388  Notify(PSTR("\r\nReport Error: "), 0x80);
389  PrintHex<uint8_t > (l2capinbuf[13], 0x80);
390  PrintHex<uint8_t > (l2capinbuf[14], 0x80);
391  }
392 #endif
393  break;
394  case 0x22: // Acknowledge output report, return function result
395 #ifdef DEBUG
396  if (l2capinbuf[13] != 0x00) { // Check if there is an error
397  Notify(PSTR("\r\nCommand failed: "), 0x80);
398  PrintHex<uint8_t > (l2capinbuf[12], 0x80);
399  }
400 #endif
401  break;
402  case 0x30: // Core buttons - (a1) 30 BB BB
403  break;
404  case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA
405  pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected
406  roll = wiimoteRoll;
407  break;
408  case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
409  break;
410  case 0x33: // Core Buttons with Accelerometer and 12 IR bytes - (a1) 33 BB BB AA AA AA II II II II II II II II II II II II
411  pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus data available
412  roll = wiimoteRoll;
413 #ifdef WIICAMERA
414  // Read the IR data
415  IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position
416  IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position
417  IR_object_s1 = (l2capinbuf[17] & 0x0F); // size value, 0-15
418 
419  IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4));
420  IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2));
421  IR_object_s2 = (l2capinbuf[20] & 0x0F);
422 
423  IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4));
424  IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2));
425  IR_object_s3 = (l2capinbuf[23] & 0x0F);
426 
427  IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4));
428  IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2));
429  IR_object_s4 = (l2capinbuf[26] & 0x0F);
430 #endif
431  break;
432  case 0x34: // Core Buttons with 19 Extension bytes - (a1) 34 BB BB EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
433  break;
434  /* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */
435  case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes
436  // (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II
437  // corresponds to output report mode 0x3e
438 
439  /**** for reading in full mode: DOES NOT WORK YET ****/
440  /* When it works it will also have intensity and bounding box data */
441  /*
442  IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
443  IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
444  IR_object_s1 = (l2capinbuf[15] & 0x0F);
445  */
446  break;
447  case 0x3F:
448  /*
449  IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
450  IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
451  IR_object_s1 = (l2capinbuf[15] & 0x0F);
452  */
453  break;
454  case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes
455  // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
456  if (motionPlusConnected) {
457  if (l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
458  if (motionValuesReset) { // We will only use the values when the gyro value has been set
459  gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)) - gyroYawZero);
460  gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero);
461  gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero);
462 
463  yawGyroSpeed = (double)gyroYawRaw / ((double)gyroYawZero / yawGyroScale);
464  rollGyroSpeed = -(double)gyroRollRaw / ((double)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values
465  pitchGyroSpeed = (double)gyroPitchRaw / ((double)gyroPitchZero / pitchGyroScale);
466 
467  /* The onboard gyro has two ranges for slow and fast mode */
468  if (!(l2capinbuf[18] & 0x02)) // Check if fast more is used
469  yawGyroSpeed *= 4.545;
470  if (!(l2capinbuf[18] & 0x01)) // Check if fast more is used
471  pitchGyroSpeed *= 4.545;
472  if (!(l2capinbuf[19] & 0x02)) // Check if fast more is used
473  rollGyroSpeed *= 4.545;
474 
475  pitch = (0.93 * (pitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimotePitch); // Use a complimentary filter to calculate the angle
476  roll = (0.93 * (roll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimoteRoll);
477 
478  gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000));
479  gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000));
480  gyroPitch += (pitchGyroSpeed * ((double)(micros() - timer) / 1000000));
481  timer = micros();
482  /*
483  // Uncomment these lines to tune the gyro scale variabels
484  Notify(PSTR("\r\ngyroYaw: "), 0x80);
485  Notify(gyroYaw, 0x80);
486  Notify(PSTR("\tgyroRoll: "), 0x80);
487  Notify(gyroRoll, 0x80);
488  Notify(PSTR("\tgyroPitch: "), 0x80);
489  Notify(gyroPitch, 0x80);
490  */
491  /*
492  Notify(PSTR("\twiimoteRoll: "), 0x80);
493  Notify(wiimoteRoll, 0x80);
494  Notify(PSTR("\twiimotePitch: "), 0x80);
495  Notify(wiimotePitch, 0x80);
496  */
497  } else {
498  if ((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values
499 #ifdef DEBUG
500  Notify(PSTR("\r\nThe gyro values has been reset"), 0x80);
501 #endif
502  gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6));
503  gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6));
504  gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6));
505 
506  rollGyroScale = 500; // You might need to adjust these
507  pitchGyroScale = 400;
508  yawGyroScale = 415;
509 
510  gyroYaw = 0;
511  gyroRoll = 0;
512  gyroPitch = 0;
513 
514  motionValuesReset = true;
515  timer = micros();
516  }
517  }
518  } else {
519  if (nunchuckConnected) {
520  hatValues[HatX] = l2capinbuf[15];
521  hatValues[HatY] = l2capinbuf[16];
522  accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416;
523  accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416;
524  accZ = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5)) - 416;
525  nunchuckPitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
526  nunchuckRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
527  }
528  //else if(classicControllerConnected) { }
529  }
530  if (l2capinbuf[19] & 0x01) {
531  if (!extensionConnected) {
532  extensionConnected = true;
533  unknownExtensionConnected = true;
534 #ifdef DEBUG
535  Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80);
536 #endif
537  }
538  } else {
539  if (extensionConnected && !unknownExtensionConnected) {
540  extensionConnected = false;
541  unknownExtensionConnected = true;
542 #ifdef DEBUG
543  Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80);
544 #endif
545  nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent
546  }
547  }
548 
549  } else if (nunchuckConnected) {
550  hatValues[HatX] = l2capinbuf[15];
551  hatValues[HatY] = l2capinbuf[16];
552  accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416;
553  accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416;
554  accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416;
555  nunchuckPitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
556  nunchuckRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
557 
558  pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected
559  roll = wiimoteRoll;
560  } else if (wiiUProControllerConnected) {
561  hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8);
562  hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8);
563  hatValues[LeftHatY] = (l2capinbuf[19] | l2capinbuf[20] << 8);
564  hatValues[RightHatY] = (l2capinbuf[21] | l2capinbuf[22] << 8);
565  }
566  break;
567 #ifdef DEBUG
568  default:
569  Notify(PSTR("\r\nUnknown Report type: "), 0x80);
570  PrintHex<uint8_t > (l2capinbuf[9], 0x80);
571  break;
572 #endif
573  }
574  }
575  }
576  }
577  L2CAP_task();
578  }
579 }
580 
581 void WII::L2CAP_task() {
582  switch (l2cap_state) {
583  /* These states are used if the Wiimote is the host */
584  case L2CAP_CONTROL_SUCCESS:
585  if (l2cap_config_success_control_flag) {
586 #ifdef DEBUG
587  Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
588 #endif
589  l2cap_state = L2CAP_INTERRUPT_SETUP;
590  }
591  break;
592 
593  case L2CAP_INTERRUPT_SETUP:
594  if (l2cap_connection_request_interrupt_flag) {
595 #ifdef DEBUG
596  Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
597 #endif
598  pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, PENDING);
599  delay(1);
600  pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL);
601  identifier++;
602  delay(1);
603  pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
604 
605  l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
606  }
607  break;
608 
609  /* These states are used if the Arduino is the host */
610  case L2CAP_CONTROL_CONNECT_REQUEST:
611  if (l2cap_connected_control_flag) {
612 #ifdef DEBUG
613  Notify(PSTR("\r\nSend HID Control Config Request"), 0x80);
614 #endif
615  identifier++;
616  pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
617  l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST;
618  }
619  break;
620 
621  case L2CAP_CONTROL_CONFIG_REQUEST:
622  if (l2cap_config_success_control_flag) {
623 #ifdef DEBUG
624  Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80);
625 #endif
626  identifier++;
627  pBtd->l2cap_connection_request(hci_handle, identifier, interrupt_dcid, HID_INTR_PSM);
628  l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST;
629  }
630  break;
631 
632  case L2CAP_INTERRUPT_CONNECT_REQUEST:
633  if (l2cap_connected_interrupt_flag) {
634 #ifdef DEBUG
635  Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80);
636 #endif
637  identifier++;
638  pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
639  l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
640  }
641  break;
642 
643  case L2CAP_INTERRUPT_CONFIG_REQUEST:
644  if (l2cap_config_success_interrupt_flag) { // Now the HID channels is established
645 #ifdef DEBUG
646  Notify(PSTR("\r\nHID Channels Established"), 0x80);
647 #endif
648  pBtd->connectToWii = false;
649  pBtd->pairWithWii = false;
650  wiimoteConnected = true;
651  stateCounter = 0;
652  l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE;
653  }
654  break;
655 
656  /* The next states are in run() */
657 
658  case L2CAP_INTERRUPT_DISCONNECT:
659  if (l2cap_disconnect_response_interrupt_flag) {
660 #ifdef DEBUG
661  Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
662 #endif
663  identifier++;
664  pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);
665  l2cap_state = L2CAP_CONTROL_DISCONNECT;
666  }
667  break;
668 
669  case L2CAP_CONTROL_DISCONNECT:
670  if (l2cap_disconnect_response_control_flag) {
671 #ifdef DEBUG
672  Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
673 #endif
674  pBtd->hci_disconnect(hci_handle);
675  hci_handle = -1; // Reset handle
676  l2cap_event_flag = 0; // Reset flags
677  l2cap_state = L2CAP_WAIT;
678  }
679  break;
680  }
681 }
682 
683 void WII::Run() {
684  switch (l2cap_state) {
685  case L2CAP_WAIT:
686  if (pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) {
687  pBtd->l2capConnectionClaimed = true;
688  activeConnection = true;
689 #ifdef DEBUG
690  Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80);
691 #endif
692  hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
693  l2cap_event_flag = 0; // Reset flags
694  identifier = 0;
695  pBtd->l2cap_connection_request(hci_handle, identifier, control_dcid, HID_CTRL_PSM);
696  l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;
697  } else if (l2cap_connection_request_control_flag) {
698 #ifdef DEBUG
699  Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
700 #endif
701  pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, PENDING);
702  delay(1);
703  pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, SUCCESSFUL);
704  identifier++;
705  delay(1);
706  pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
707  l2cap_state = L2CAP_CONTROL_SUCCESS;
708  }
709  break;
710 
711  case L2CAP_CHECK_MOTION_PLUS_STATE:
712 #ifdef DEBUG
713  if (stateCounter == 0) // Only print onnce
714  Notify(PSTR("\r\nChecking if a Motion Plus is connected"), 0x80);
715 #endif
716  stateCounter++;
717  if (stateCounter % 200 == 0)
718  checkMotionPresent(); // Check if there is a motion plus connected
719  if (motion_plus_connected_flag) {
720  stateCounter = 0;
721  l2cap_state = L2CAP_INIT_MOTION_PLUS_STATE;
722  timer = micros();
723 
724  if (unknownExtensionConnected) {
725 #ifdef DEBUG
726  Notify(PSTR("\r\nA extension is also connected"), 0x80);
727 #endif
728  activateNunchuck = true; // For we will just set this to true as this the only extension supported so far
729  }
730 
731  } else if (stateCounter == 601) { // We will try three times to check for the motion plus
732 #ifdef DEBUG
733  Notify(PSTR("\r\nNo Motion Plus was detected"), 0x80);
734 #endif
735  stateCounter = 0;
736  l2cap_state = L2CAP_CHECK_EXTENSION_STATE;
737  }
738  break;
739 
740  case L2CAP_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port
741 #ifdef DEBUG
742  if (stateCounter == 0) // Only print onnce
743  Notify(PSTR("\r\nChecking if there is any extension connected"), 0x80);
744 #endif
745  stateCounter++; // We use this counter as there has to be a short delay between the commands
746  if (stateCounter == 1)
747  statusRequest(); // See if a new device has connected
748  if (stateCounter == 100) {
749  if (unknownExtensionConnected) // Check if there is a extension is connected to the port
750  initExtension1();
751  else
752  stateCounter = 399;
753  } else if (stateCounter == 200)
754  initExtension2();
755  else if (stateCounter == 300) {
756  readExtensionType();
757  unknownExtensionConnected = false;
758  } else if (stateCounter == 400) {
759  stateCounter = 0;
760  l2cap_state = L2CAP_LED_STATE;
761  }
762  break;
763 
764  case L2CAP_INIT_MOTION_PLUS_STATE:
765  stateCounter++;
766  if (stateCounter == 1)
767  initMotionPlus();
768  else if (stateCounter == 100)
769  activateMotionPlus();
770  else if (stateCounter == 200)
771  readExtensionType(); // Check if it has been activated
772  else if (stateCounter == 300) {
773  stateCounter = 0;
774  unknownExtensionConnected = false; // The motion plus will send a status report when it's activated, we will set this to false so it doesn't reinitialize the Motion Plus
775  l2cap_state = L2CAP_LED_STATE;
776  }
777  break;
778 
779  case L2CAP_LED_STATE:
780  if (nunchuck_connected_flag)
781  nunchuckConnected = true;
782  setLedStatus();
783  l2cap_state = L2CAP_DONE;
784  break;
785 
786  case L2CAP_DONE:
787  if (unknownExtensionConnected) {
788 #ifdef DEBUG
789  if (stateCounter == 0) // Only print once
790  Notify(PSTR("\r\nChecking extension port"), 0x80);
791 #endif
792  stateCounter++; // We will use this counter as there has to be a short delay between the commands
793  if (stateCounter == 50)
794  statusRequest();
795  else if (stateCounter == 100)
796  initExtension1();
797  else if (stateCounter == 150)
798  if ((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected))
799  initExtension2();
800  else
801  stateCounter = 299; // There is no extension connected
802  else if (stateCounter == 200)
803  readExtensionType();
804  else if (stateCounter == 250) {
805  if (nunchuck_connected_flag) {
806 #ifdef DEBUG
807  Notify(PSTR("\r\nNunchuck was reconnected"), 0x80);
808 #endif
809  activateNunchuck = true;
810  nunchuckConnected = true;
811  }
812  if (!motionPlusConnected)
813  stateCounter = 449;
814  } else if (stateCounter == 300) {
815  if (motionPlusConnected) {
816 #ifdef DEBUG
817  Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80);
818 #endif
819  initMotionPlus();
820  } else
821  stateCounter = 449;
822  } else if (stateCounter == 350)
823  activateMotionPlus();
824  else if (stateCounter == 400)
825  readExtensionType(); // Check if it has been activated
826  else if (stateCounter == 450) {
827  //setLedStatus();
828  stateCounter = 0;
829  unknownExtensionConnected = false;
830  }
831  } else
832  stateCounter = 0;
833  break;
834  }
835 }
836 
837 /************************************************************/
838 /* HID Commands */
839 
840 /************************************************************/
841 void WII::HID_Command(uint8_t* data, uint8_t nbytes) {
842  if (pBtd->motionPlusInside)
843  pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // It's the new wiimote with the Motion Plus Inside
844  else
845  pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]);
846 }
847 
848 void WII::setAllOff() {
849  HIDBuffer[1] = 0x11;
850  HIDBuffer[2] = 0x00;
851  HID_Command(HIDBuffer, 3);
852 }
853 
854 void WII::setRumbleOff() {
855  HIDBuffer[1] = 0x11;
856  HIDBuffer[2] &= ~0x01; // Bit 0 control the rumble
857  HID_Command(HIDBuffer, 3);
858 }
859 
860 void WII::setRumbleOn() {
861  HIDBuffer[1] = 0x11;
862  HIDBuffer[2] |= 0x01; // Bit 0 control the rumble
863  HID_Command(HIDBuffer, 3);
864 }
865 
866 void WII::setRumbleToggle() {
867  HIDBuffer[1] = 0x11;
868  HIDBuffer[2] ^= 0x01; // Bit 0 control the rumble
869  HID_Command(HIDBuffer, 3);
870 }
871 
872 void WII::setLedRaw(uint8_t value) {
873  HIDBuffer[1] = 0x11;
874  HIDBuffer[2] = value | (HIDBuffer[2] & 0x01); // Keep the rumble bit
875  HID_Command(HIDBuffer, 3);
876 }
877 void WII::setLedOff(LED a) {
878  HIDBuffer[1] = 0x11;
879  HIDBuffer[2] &= ~(pgm_read_byte(&LEDS[(uint8_t)a]));
880  HID_Command(HIDBuffer, 3);
881 }
882 
883 void WII::setLedOn(LED a) {
884  HIDBuffer[1] = 0x11;
885  HIDBuffer[2] |= pgm_read_byte(&LEDS[(uint8_t)a]);
886  HID_Command(HIDBuffer, 3);
887 }
888 
889 void WII::setLedToggle(LED a) {
890  HIDBuffer[1] = 0x11;
891  HIDBuffer[2] ^= pgm_read_byte(&LEDS[(uint8_t)a]);
892  HID_Command(HIDBuffer, 3);
893 }
894 
895 void WII::setLedStatus() {
896  HIDBuffer[1] = 0x11;
897  HIDBuffer[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit
898  if (wiimoteConnected)
899  HIDBuffer[2] |= 0x10; // If it's connected LED1 will light up
900  if (motionPlusConnected)
901  HIDBuffer[2] |= 0x20; // If it's connected LED2 will light up
902  if (nunchuckConnected)
903  HIDBuffer[2] |= 0x40; // If it's connected LED3 will light up
904 
905  HID_Command(HIDBuffer, 3);
906 }
907 
908 void WII::setReportMode(bool continuous, uint8_t mode) {
909  uint8_t cmd_buf[4];
910  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
911  cmd_buf[1] = 0x12;
912  if (continuous)
913  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit
914  else
915  cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Keep the rumble bit
916  cmd_buf[3] = mode;
917  HID_Command(cmd_buf, 4);
918 }
919 
920 void WII::statusRequest() {
921  uint8_t cmd_buf[3];
922  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
923  cmd_buf[1] = 0x15;
924  cmd_buf[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit
925  HID_Command(cmd_buf, 3);
926 }
927 
928 /************************************************************/
929 /* Memmory Commands */
930 
931 /************************************************************/
932 void WII::writeData(uint32_t offset, uint8_t size, uint8_t* data) {
933  uint8_t cmd_buf[23];
934  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
935  cmd_buf[1] = 0x16; // Write data
936  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Write to memory, clear bit 2 to write to EEPROM
937  cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16);
938  cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8);
939  cmd_buf[5] = (uint8_t)(offset & 0xFF);
940  cmd_buf[6] = size;
941  uint8_t i = 0;
942  for (; i < size; i++)
943  cmd_buf[7 + i] = data[i];
944  for (; i < 16; i++) // Set the rest to zero
945  cmd_buf[7 + i] = 0x00;
946  HID_Command(cmd_buf, 23);
947 }
948 
949 void WII::initExtension1() {
950  uint8_t buf[1];
951  buf[0] = 0x55;
952  writeData(0xA400F0, 1, buf);
953 }
954 
955 void WII::initExtension2() {
956  uint8_t buf[1];
957  buf[0] = 0x00;
958  writeData(0xA400FB, 1, buf);
959 }
960 
961 void WII::initMotionPlus() {
962  uint8_t buf[1];
963  buf[0] = 0x55;
964  writeData(0xA600F0, 1, buf);
965 }
966 
967 void WII::activateMotionPlus() {
968  uint8_t buf[1];
969  if (pBtd->wiiUProController) {
970 #ifdef DEBUG
971  Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80);
972 #endif
973  buf[0] = 0x00; // It seems like you can send anything but 0x04, 0x05, and 0x07
974  } else if (activateNunchuck) {
975 #ifdef DEBUG
976  Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"), 0x80);
977 #endif
978  buf[0] = 0x05; // Activate nunchuck pass-through mode
979  } //else if(classicControllerConnected && extensionConnected)
980  //buf[0] = 0x07;
981  else {
982 #ifdef DEBUG
983  Notify(PSTR("\r\nActivating Motion Plus in normal mode"), 0x80);
984 #endif
985  buf[0] = 0x04; // Don't use any extension
986  }
987  writeData(0xA600FE, 1, buf);
988 }
989 
990 void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) {
991  uint8_t cmd_buf[8];
992  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
993  cmd_buf[1] = 0x17; // Read data
994  if (EEPROM)
995  cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Read from EEPROM
996  else
997  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory
998  cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16);
999  cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8);
1000  cmd_buf[5] = (uint8_t)(offset & 0xFF);
1001  cmd_buf[6] = (uint8_t)((size & 0xFF00) >> 8);
1002  cmd_buf[7] = (uint8_t)(size & 0xFF);
1003 
1004  HID_Command(cmd_buf, 8);
1005 }
1006 
1007 void WII::readExtensionType() {
1008  readData(0xA400FA, 6, false);
1009 }
1010 
1011 void WII::readCalData() {
1012  readData(0x0016, 8, true);
1013 }
1014 
1015 void WII::checkMotionPresent() {
1016  readData(0xA600FA, 6, false);
1017 }
1018 
1019 /************************************************************/
1020 /* WII Commands */
1021 
1022 /************************************************************/
1023 
1024 bool WII::getButtonPress(Button b) { // Return true when a button is pressed
1025  if (wiiUProControllerConnected)
1026  return (ButtonState & pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b]));
1027  else
1028  return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b]));
1029 }
1030 
1031 bool WII::getButtonClick(Button b) { // Only return true when a button is clicked
1032  uint32_t button;
1033  if (wiiUProControllerConnected)
1034  button = pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b]);
1035  else
1036  button = pgm_read_dword(&BUTTONS[(uint8_t)b]);
1037  bool click = (ButtonClickState & button);
1038  ButtonClickState &= ~button; // clear "click" event
1039  return click;
1040 }
1041 
1042 uint8_t WII::getAnalogHat(Hat a) {
1043  if (!nunchuckConnected)
1044  return 127; // Return center position
1045  else {
1046  uint8_t output = hatValues[(uint8_t)a];
1047  if (output == 0xFF || output == 0x00) // The joystick will only read 255 or 0 when the cable is unplugged or initializing, so we will just return the center position
1048  return 127;
1049  else
1050  return output;
1051  }
1052 }
1053 
1054 uint16_t WII::getAnalogHat(AnalogHat a) {
1055  if (!wiiUProControllerConnected)
1056  return 2000;
1057  else {
1058  uint16_t output = hatValues[(uint8_t)a];
1059  if (output == 0x00) // The joystick will only read 0 when it is first initializing, so we will just return the center position
1060  return 2000;
1061  else
1062  return output;
1063  }
1064 }
1065 /************************************************************/
1066 /* The following functions are for the IR camera */
1067 /************************************************************/
1068 
1069 #ifdef WIICAMERA
1070 
1071 void WII::IRinitialize() { // Turns on and initialises the IR camera
1072 
1073  enableIRCamera1();
1074 #ifdef DEBUG
1075  Notify(PSTR("\r\nEnable IR Camera1 Complete"), 0x80);
1076 #endif
1077  delay(80);
1078 
1079  enableIRCamera2();
1080 #ifdef DEBUG
1081  Notify(PSTR("\r\nEnable IR Camera2 Complete"), 0x80);
1082 #endif
1083  delay(80);
1084 
1085  write0x08Value();
1086 #ifdef DEBUG
1087  Notify(PSTR("\r\nWrote hex number 0x08"), 0x80);
1088 #endif
1089  delay(80);
1090 
1091  writeSensitivityBlock1();
1092 #ifdef DEBUG
1093  Notify(PSTR("\r\nWrote Sensitivity Block 1"), 0x80);
1094 #endif
1095  delay(80);
1096 
1097  writeSensitivityBlock2();
1098 #ifdef DEBUG
1099  Notify(PSTR("\r\nWrote Sensitivity Block 2"), 0x80);
1100 #endif
1101  delay(80);
1102 
1103  uint8_t mode_num = 0x03;
1104  setWiiModeNumber(mode_num); // Change input for whatever mode you want i.e. 0x01, 0x03, or 0x05
1105 #ifdef DEBUG
1106  Notify(PSTR("\r\nSet Wii Mode Number To 0x"), 0x80);
1107  PrintHex<uint8_t > (mode_num, 0x80);
1108 #endif
1109  delay(80);
1110 
1111  write0x08Value();
1112 #ifdef DEBUG
1113  Notify(PSTR("\r\nWrote Hex Number 0x08"), 0x80);
1114 #endif
1115  delay(80);
1116 
1117  setReportMode(false, 0x33);
1118  //setReportMode(false, 0x3f); // For full reporting mode, doesn't work yet
1119 #ifdef DEBUG
1120  Notify(PSTR("\r\nSet Report Mode to 0x33"), 0x80);
1121 #endif
1122  delay(80);
1123 
1124  statusRequest(); // Used to update wiiState - call isIRCameraEnabled() afterwards to check if it actually worked
1125 #ifdef DEBUG
1126  Notify(PSTR("\r\nIR Initialized"), 0x80);
1127 #endif
1128 }
1129 
1130 void WII::enableIRCamera1() {
1131  uint8_t cmd_buf[3];
1132  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
1133  cmd_buf[1] = 0x13; // Output report 13
1134  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
1135  HID_Command(cmd_buf, 3);
1136 }
1137 
1138 void WII::enableIRCamera2() {
1139  uint8_t cmd_buf[3];
1140  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
1141  cmd_buf[1] = 0x1A; // Output report 1A
1142  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
1143  HID_Command(cmd_buf, 3);
1144 }
1145 
1146 void WII::writeSensitivityBlock1() {
1147  uint8_t buf[9];
1148  buf[0] = 0x00;
1149  buf[1] = 0x00;
1150  buf[2] = 0x00;
1151  buf[3] = 0x00;
1152  buf[4] = 0x00;
1153  buf[5] = 0x00;
1154  buf[6] = 0x90;
1155  buf[7] = 0x00;
1156  buf[8] = 0x41;
1157 
1158  writeData(0xB00000, 9, buf);
1159 }
1160 
1161 void WII::writeSensitivityBlock2() {
1162  uint8_t buf[2];
1163  buf[0] = 0x40;
1164  buf[1] = 0x00;
1165 
1166  writeData(0xB0001A, 2, buf);
1167 }
1168 
1169 void WII::write0x08Value() {
1170  uint8_t cmd = 0x08;
1171  writeData(0xb00030, 1, &cmd);
1172 }
1173 
1174 void WII::setWiiModeNumber(uint8_t mode_number) { // mode_number in hex i.e. 0x03 for extended mode
1175  writeData(0xb00033, 1, &mode_number);
1176 }
1177 #endif
Generated by   doxygen 1.8.3.1