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