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