USB Host Shield 2.0
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 BluetoothService(p) // Pointer to USB class instance - mandatory
87 {
89 
90  HIDBuffer[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
91 
92  /* Set device cid for the control and intterrupt channelse - LSB */
93  control_dcid[0] = 0x60; // 0x0060
94  control_dcid[1] = 0x00;
95  interrupt_dcid[0] = 0x61; // 0x0061
96  interrupt_dcid[1] = 0x00;
97 
98  Reset();
99 }
100 
101 void WII::Reset() {
102  wiimoteConnected = false;
103  nunchuckConnected = false;
104  motionPlusConnected = false;
105  activateNunchuck = false;
106  motionValuesReset = false;
107  activeConnection = false;
108  motionPlusInside = false;
109  pBtd->wiiUProController = false;
111  wiiBalanceBoardConnected = false;
112  l2cap_event_flag = 0; // Reset flags
113  l2cap_state = L2CAP_WAIT;
114 }
115 
116 void WII::disconnect() { // Use this void to disconnect any of the controllers
117  if(!motionPlusInside) { // The old Wiimote needs a delay after the first command or it will automatically reconnect
118  if(motionPlusConnected) {
119 #ifdef DEBUG_USB_HOST
120  Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80);
121 #endif
122  initExtension1(); // This will disable the Motion Plus extension
123  }
124  timer = millis() + 1000; // We have to wait for the message before the rest of the channels can be deactivated
125  } else
126  timer = millis(); // Don't wait
127  // First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection
128  pBtd->l2cap_disconnection_request(hci_handle, ++identifier, interrupt_scid, interrupt_dcid);
129  Reset();
130  l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
131 }
132 
133 void WII::ACLData(uint8_t* l2capinbuf) {
134  if(!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) {
135  if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
136  if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
137  motionPlusInside = pBtd->motionPlusInside;
138  pBtd->incomingWii = false;
139  pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
140  activeConnection = true;
141  hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
142  l2cap_state = L2CAP_WAIT;
143  }
144  }
145  }
146 
147  if(checkHciHandle(l2capinbuf, hci_handle)) { // acl_handle_ok
148  if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { // 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]) {
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];
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];
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];
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];
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];
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];
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];
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];
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));
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 
300  if(!checkBatteryLevel) { // If this is true it means that the user must have called getBatteryLevel()
301  if(l2capinbuf[12] & 0x02) { // Check if a extension is connected
302 #ifdef DEBUG_USB_HOST
303  if(!unknownExtensionConnected)
304  Notify(PSTR("\r\nExtension connected"), 0x80);
305 #endif
306  unknownExtensionConnected = true;
307 #ifdef WIICAMERA
308  if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
309 #endif
310  setReportMode(false, 0x35); // Also read the extension
311  } else {
312 #ifdef DEBUG_USB_HOST
313  Notify(PSTR("\r\nExtension disconnected"), 0x80);
314 #endif
315  if(motionPlusConnected) {
316 #ifdef DEBUG_USB_HOST
317  Notify(PSTR(" - from Motion Plus"), 0x80);
318 #endif
320  if(!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false
321  nunchuckConnected = false;
322  //else if(classicControllerConnected)
323  } else if(nunchuckConnected) {
324 #ifdef DEBUG_USB_HOST
325  Notify(PSTR(" - Nunchuck"), 0x80);
326 #endif
327  nunchuckConnected = false; // It must be the Nunchuck controller then
329  onInit();
330  setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
331  } else
332  setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
333  }
334  }
335  else {
336 #ifdef EXTRADEBUG
337  Notify(PSTR("\r\nChecking battery level"), 0x80);
338 #endif
339  checkBatteryLevel = false; // Check for extensions by default
340  }
341 #ifdef DEBUG_USB_HOST
342  if(l2capinbuf[12] & 0x01)
343  Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80);
344 #endif
345 
346  break;
347  case 0x21: // Read Memory Data
348  if((l2capinbuf[12] & 0x0F) == 0) { // No error
349  uint8_t reportLength = (l2capinbuf[12] >> 4) + 1; // // Bit 4-7 is the length - 1
350  // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
351  if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
352 #ifdef DEBUG_USB_HOST
353  Notify(PSTR("\r\nNunchuck connected"), 0x80);
354 #endif
356  } else if(l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) {
357 #ifdef DEBUG_USB_HOST
358  Notify(PSTR("\r\nMotion Plus connected"), 0x80);
359 #endif
361  } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) {
362 #ifdef DEBUG_USB_HOST
363  Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80);
364 #endif
365  motionPlusConnected = true;
366 #ifdef WIICAMERA
367  if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
368 #endif
369  setReportMode(false, 0x35); // Also read the extension
370  } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
371 #ifdef DEBUG_USB_HOST
372  Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80);
373 #endif
374  activateNunchuck = false;
375  motionPlusConnected = true;
376  nunchuckConnected = true;
377 #ifdef WIICAMERA
378  if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
379 #endif
380  setReportMode(false, 0x35); // Also read the extension
381  } 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) {
382 #ifdef DEBUG_USB_HOST
383  Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80);
384  Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80);
385 #endif
386  stateCounter = 300; // Skip the rest in "WII_CHECK_MOTION_PLUS_STATE"
387  } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) {
388 #ifdef DEBUG_USB_HOST
389  Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80);
390 #endif
392  } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x02) {
393 #ifdef DEBUG_USB_HOST
394  Notify(PSTR("\r\nWii Balance Board connected"), 0x80);
395 #endif
396  setReportMode(false, 0x32); // Read the Wii Balance Board extension
398  }
399  // Wii Balance Board calibration reports (24 bits in total)
400  else if(l2capinbuf[13] == 0x00 && l2capinbuf[14] == 0x24 && reportLength == 16) { // First 16-bit
401  for(uint8_t i = 0; i < 2; i++) {
402  for(uint8_t j = 0; j < 4; j++)
403  wiiBalanceBoardCal[i][j] = l2capinbuf[16 + 8 * i + 2 * j] | l2capinbuf[15 + 8 * i + 2 * j] << 8;
404  }
405  } else if(l2capinbuf[13] == 0x00 && l2capinbuf[14] == 0x34 && reportLength == 8) { // Last 8-bit
406  for(uint8_t j = 0; j < 4; j++)
407  wiiBalanceBoardCal[2][j] = l2capinbuf[16 + 2 * j] | l2capinbuf[15 + 2 * j] << 8;
408 #ifdef DEBUG_USB_HOST
409  Notify(PSTR("\r\nWii Balance Board calibration values read successfully"), 0x80);
410 #endif
413  }
414 #ifdef DEBUG_USB_HOST
415  else {
416  Notify(PSTR("\r\nUnknown Device: "), 0x80);
417  D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
418  D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
419  Notify(PSTR("\r\nData: "), 0x80);
420  for(uint8_t i = 0; i < reportLength; i++) {
421  D_PrintHex<uint8_t > (l2capinbuf[15 + i], 0x80);
422  Notify(PSTR(" "), 0x80);
423  }
424  }
425 #endif
426  }
427 #ifdef EXTRADEBUG
428  else {
429  Notify(PSTR("\r\nReport Error: "), 0x80);
430  D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
431  D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
432  }
433 #endif
434  break;
435  case 0x22: // Acknowledge output report, return function result
436 #ifdef DEBUG_USB_HOST
437  if(l2capinbuf[13] != 0x00) { // Check if there is an error
438  Notify(PSTR("\r\nCommand failed: "), 0x80);
439  D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
440  }
441 #endif
442  break;
443  case 0x30: // Core buttons - (a1) 30 BB BB
444  break;
445  case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA
446  break;
447  case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
448  // See: http://wiibrew.org/wiki/Wii_Balance_Board#Data_Format
449  wiiBalanceBoardRaw[TopRight] = l2capinbuf[13] | l2capinbuf[12] << 8; // Top right
450  wiiBalanceBoardRaw[BotRight] = l2capinbuf[15] | l2capinbuf[14] << 8; // Bottom right
451  wiiBalanceBoardRaw[TopLeft] = l2capinbuf[17] | l2capinbuf[16] << 8; // Top left
452  wiiBalanceBoardRaw[BotLeft] = l2capinbuf[19] | l2capinbuf[18] << 8; // Bottom left
453  break;
454  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
455 #ifdef WIICAMERA
456  // Read the IR data
457  IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position
458  IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position
459  IR_object_s1 = (l2capinbuf[17] & 0x0F); // Size value, 0-15
460 
461  IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4));
462  IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2));
463  IR_object_s2 = (l2capinbuf[20] & 0x0F);
464 
465  IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4));
466  IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2));
467  IR_object_s3 = (l2capinbuf[23] & 0x0F);
468 
469  IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4));
470  IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2));
471  IR_object_s4 = (l2capinbuf[26] & 0x0F);
472 #endif
473  break;
474  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
475  break;
476  /* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */
477  case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes
478  // (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
479  // corresponds to output report mode 0x3e
480 
481  /**** for reading in full mode: DOES NOT WORK YET ****/
482  /* When it works it will also have intensity and bounding box data */
483  /*
484  IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
485  IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
486  IR_object_s1 = (l2capinbuf[15] & 0x0F);
487  */
488  break;
489  case 0x3F:
490  /*
491  IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
492  IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
493  IR_object_s1 = (l2capinbuf[15] & 0x0F);
494  */
495  break;
496  case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes
497  // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
498 #if 1 // Set this to 0 if you don't want to use an extension, this reduceds the size of the library a lot!
499  if(motionPlusConnected) {
500  if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
501  if(motionValuesReset) { // We will only use the values when the gyro value has been set
502  gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)) - gyroYawZero);
503  gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero);
504  gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero);
505 
506  yawGyroSpeed = (float)gyroYawRaw / ((float)gyroYawZero / yawGyroScale);
507  rollGyroSpeed = -(float)gyroRollRaw / ((float)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values
509 
510  /* The onboard gyro has two ranges for slow and fast mode */
511  if(!(l2capinbuf[18] & 0x02)) // Check if fast mode is used
512  yawGyroSpeed *= 4.545;
513  if(!(l2capinbuf[18] & 0x01)) // Check if fast mode is used
514  pitchGyroSpeed *= 4.545;
515  if(!(l2capinbuf[19] & 0x02)) // Check if fast mode is used
516  rollGyroSpeed *= 4.545;
517 
518  compPitch = (0.93f * (compPitch + (pitchGyroSpeed * (float)(micros() - timer) / 1000000.0f)))+(0.07f * getWiimotePitch()); // Use a complimentary filter to calculate the angle
519  compRoll = (0.93f * (compRoll + (rollGyroSpeed * (float)(micros() - timer) / 1000000.0f)))+(0.07f * getWiimoteRoll());
520 
521  gyroYaw += (yawGyroSpeed * ((float)(micros() - timer) / 1000000.0f));
522  gyroRoll += (rollGyroSpeed * ((float)(micros() - timer) / 1000000.0f));
523  gyroPitch += (pitchGyroSpeed * ((float)(micros() - timer) / 1000000.0f));
524  timer = micros();
525  /*
526  // Uncomment these lines to tune the gyro scale variabels
527  Notify(PSTR("\r\ngyroYaw: "), 0x80);
528  Notify(gyroYaw, 0x80);
529  Notify(PSTR("\tgyroRoll: "), 0x80);
530  Notify(gyroRoll, 0x80);
531  Notify(PSTR("\tgyroPitch: "), 0x80);
532  Notify(gyroPitch, 0x80);
533  */
534  /*
535  Notify(PSTR("\twiimoteRoll: "), 0x80);
536  Notify(wiimoteRoll, 0x80);
537  Notify(PSTR("\twiimotePitch: "), 0x80);
538  Notify(wiimotePitch, 0x80);
539  */
540  } else {
541  if((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values
542 #ifdef DEBUG_USB_HOST
543  Notify(PSTR("\r\nThe gyro values has been reset"), 0x80);
544 #endif
545  gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6));
546  gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6));
547  gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6));
548 
549  rollGyroScale = 500; // You might need to adjust these
550  pitchGyroScale = 400;
551  yawGyroScale = 415;
552 
553  gyroYaw = 0;
554  gyroRoll = 0;
555  gyroPitch = 0;
556 
557  motionValuesReset = true;
558  timer = micros();
559  }
560  }
561  } else {
562  if(nunchuckConnected) {
563  hatValues[HatX] = l2capinbuf[15];
564  hatValues[HatY] = l2capinbuf[16];
565  accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416;
566  accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416;
567  accZnunchuck = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5)) - 416;
568  }
569  //else if(classicControllerConnected) { }
570  }
571  if(l2capinbuf[19] & 0x01) {
572  if(!extensionConnected) {
573  extensionConnected = true;
574  unknownExtensionConnected = true;
575 #ifdef DEBUG_USB_HOST
576  Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80);
577 #endif
578  }
579  } else {
580  if(extensionConnected && !unknownExtensionConnected) {
581  extensionConnected = false;
582  unknownExtensionConnected = true;
583 #ifdef DEBUG_USB_HOST
584  Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80);
585 #endif
586  nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent
587  }
588  }
589 
590  } else if(nunchuckConnected) {
591  hatValues[HatX] = l2capinbuf[15];
592  hatValues[HatY] = l2capinbuf[16];
593  accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416;
594  accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416;
595  accZnunchuck = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416;
596  } else if(wiiUProControllerConnected) {
597  hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8);
598  hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8);
599  hatValues[LeftHatY] = (l2capinbuf[19] | l2capinbuf[20] << 8);
600  hatValues[RightHatY] = (l2capinbuf[21] | l2capinbuf[22] << 8);
601  }
602 #endif
603  break;
604 #ifdef DEBUG_USB_HOST
605  default:
606  Notify(PSTR("\r\nUnknown Report type: "), 0x80);
607  D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
608  break;
609 #endif
610  }
611  }
612  }
613  L2CAP_task();
614  }
615 }
616 
617 void WII::L2CAP_task() {
618  switch(l2cap_state) {
619  /* These states are used if the Wiimote is the host */
622 #ifdef DEBUG_USB_HOST
623  Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
624 #endif
625  l2cap_state = L2CAP_INTERRUPT_SETUP;
626  }
627  break;
628 
631 #ifdef DEBUG_USB_HOST
632  Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
633 #endif
634  pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, PENDING);
635  delay(1);
636  pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL);
637  identifier++;
638  delay(1);
639  pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
640 
641  l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
642  }
643  break;
644 
645  /* These states are used if the Arduino is the host */
648 #ifdef DEBUG_USB_HOST
649  Notify(PSTR("\r\nSend HID Control Config Request"), 0x80);
650 #endif
651  identifier++;
653  l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST;
654  }
655  break;
656 
659 #ifdef DEBUG_USB_HOST
660  Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80);
661 #endif
662  identifier++;
664  l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST;
665  }
666  break;
667 
670 #ifdef DEBUG_USB_HOST
671  Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80);
672 #endif
673  identifier++;
674  pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
675  l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
676  }
677  break;
678 
680  if(l2cap_check_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)) { // Now the HID channels is established
681 #ifdef DEBUG_USB_HOST
682  Notify(PSTR("\r\nHID Channels Established"), 0x80);
683 #endif
684  pBtd->connectToWii = false;
685  pBtd->pairWithWii = false;
686  stateCounter = 0;
687  l2cap_state = WII_CHECK_MOTION_PLUS_STATE;
688  }
689  break;
690 
691  /* The next states are in run() */
692 
694  if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE) && ((long)(millis() - timer) >= 0L)) {
695 #ifdef DEBUG_USB_HOST
696  Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
697 #endif
698  identifier++;
699  pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);
700  l2cap_state = L2CAP_CONTROL_DISCONNECT;
701  }
702  break;
703 
706 #ifdef DEBUG_USB_HOST
707  Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
708 #endif
710  hci_handle = -1; // Reset handle
711  l2cap_event_flag = 0; // Reset flags
712  l2cap_state = L2CAP_WAIT;
713  }
714  break;
715  }
716 }
717 
718 void WII::Run() {
719  if(l2cap_state == L2CAP_INTERRUPT_DISCONNECT && ((long)(millis() - timer) >= 0L))
720  L2CAP_task(); // Call the rest of the disconnection routine after we have waited long enough
721 
722  switch(l2cap_state) {
723  case L2CAP_WAIT:
724  if(pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) {
726  activeConnection = true;
727  motionPlusInside = pBtd->motionPlusInside;
728 #ifdef DEBUG_USB_HOST
729  Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80);
730 #endif
731  hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
732  l2cap_event_flag = 0; // Reset flags
733  identifier = 0;
735  l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;
737 #ifdef DEBUG_USB_HOST
738  Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
739 #endif
740  pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, PENDING);
741  delay(1);
742  pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, SUCCESSFUL);
743  identifier++;
744  delay(1);
746  l2cap_state = L2CAP_CONTROL_SUCCESS;
747  }
748  break;
749 
751 #ifdef DEBUG_USB_HOST
752  if(stateCounter == 0) // Only print onnce
753  Notify(PSTR("\r\nChecking if a Motion Plus is connected"), 0x80);
754 #endif
755  stateCounter++;
756  if(stateCounter % 200 == 0)
757  checkMotionPresent(); // Check if there is a motion plus connected
759  stateCounter = 0;
760  l2cap_state = WII_INIT_MOTION_PLUS_STATE;
761  timer = micros();
762 
763  if(unknownExtensionConnected) {
764 #ifdef DEBUG_USB_HOST
765  Notify(PSTR("\r\nA extension is also connected"), 0x80);
766 #endif
767  activateNunchuck = true; // For we will just set this to true as this the only extension supported so far
768  }
769 
770  } else if(stateCounter == 601) { // We will try three times to check for the motion plus
771 #ifdef DEBUG_USB_HOST
772  Notify(PSTR("\r\nNo Motion Plus was detected"), 0x80);
773 #endif
774  stateCounter = 0;
775  l2cap_state = WII_CHECK_EXTENSION_STATE;
776  }
777  break;
778 
779  case WII_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port
780 #ifdef DEBUG_USB_HOST
781  if(stateCounter == 0) // Only print onnce
782  Notify(PSTR("\r\nChecking if there is any extension connected"), 0x80);
783 #endif
784  stateCounter++; // We use this counter as there has to be a short delay between the commands
785  if(stateCounter == 1)
786  statusRequest(); // See if a new device has connected
787  if(stateCounter == 100) {
788  if(unknownExtensionConnected) // Check if there is a extension is connected to the port
789  initExtension1();
790  else
791  stateCounter = 499;
792  } else if(stateCounter == 200)
793  initExtension2();
794  else if(stateCounter == 300) {
795  readExtensionType();
796  unknownExtensionConnected = false;
797  } else if(stateCounter == 400) {
799 #ifdef DEBUG_USB_HOST
800  Notify(PSTR("\r\nReading Wii Balance Board calibration values"), 0x80);
801 #endif
802  readWiiBalanceBoardCalibration();
803  } else
804  stateCounter = 499;
805  } else if(stateCounter == 500) {
806  stateCounter = 0;
807  l2cap_state = TURN_ON_LED;
808  }
809  break;
810 
812  stateCounter++;
813  if(stateCounter == 1)
814  initMotionPlus();
815  else if(stateCounter == 100)
816  activateMotionPlus();
817  else if(stateCounter == 200)
818  readExtensionType(); // Check if it has been activated
819  else if(stateCounter == 300) {
820  stateCounter = 0;
821  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
822  l2cap_state = TURN_ON_LED;
823  }
824  break;
825 
826  case TURN_ON_LED:
828  nunchuckConnected = true;
829  wiimoteConnected = true;
830  onInit();
831  l2cap_state = L2CAP_DONE;
832  break;
833 
834  case L2CAP_DONE:
835  if(unknownExtensionConnected) {
836 #ifdef DEBUG_USB_HOST
837  if(stateCounter == 0) // Only print once
838  Notify(PSTR("\r\nChecking extension port"), 0x80);
839 #endif
840  stateCounter++; // We will use this counter as there has to be a short delay between the commands
841  if(stateCounter == 50)
842  statusRequest();
843  else if(stateCounter == 100)
844  initExtension1();
845  else if(stateCounter == 150)
846  if((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected))
847  initExtension2();
848  else
849  stateCounter = 299; // There is no extension connected
850  else if(stateCounter == 200)
851  readExtensionType();
852  else if(stateCounter == 250) {
854 #ifdef DEBUG_USB_HOST
855  Notify(PSTR("\r\nNunchuck was reconnected"), 0x80);
856 #endif
857  activateNunchuck = true;
858  nunchuckConnected = true;
859  }
861  stateCounter = 449;
862  } else if(stateCounter == 300) {
863  if(motionPlusConnected) {
864 #ifdef DEBUG_USB_HOST
865  Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80);
866 #endif
867  initMotionPlus();
868  } else
869  stateCounter = 449;
870  } else if(stateCounter == 350)
871  activateMotionPlus();
872  else if(stateCounter == 400)
873  readExtensionType(); // Check if it has been activated
874  else if(stateCounter == 450) {
875  onInit();
876  stateCounter = 0;
877  unknownExtensionConnected = false;
878  }
879  } else
880  stateCounter = 0;
881  break;
882  }
883 }
884 
885 /************************************************************/
886 /* HID Commands */
887 /************************************************************/
888 
889 void WII::HID_Command(uint8_t* data, uint8_t nbytes) {
890  if(motionPlusInside)
891  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
892  else
893  pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]);
894 }
895 
897  HIDBuffer[1] = 0x11;
898  HIDBuffer[2] = 0x00;
899  HID_Command(HIDBuffer, 3);
900 }
901 
903  HIDBuffer[1] = 0x11;
904  HIDBuffer[2] &= ~0x01; // Bit 0 control the rumble
905  HID_Command(HIDBuffer, 3);
906 }
907 
909  HIDBuffer[1] = 0x11;
910  HIDBuffer[2] |= 0x01; // Bit 0 control the rumble
911  HID_Command(HIDBuffer, 3);
912 }
913 
915  HIDBuffer[1] = 0x11;
916  HIDBuffer[2] ^= 0x01; // Bit 0 control the rumble
917  HID_Command(HIDBuffer, 3);
918 }
919 
920 void WII::setLedRaw(uint8_t value) {
921  HIDBuffer[1] = 0x11;
922  HIDBuffer[2] = value | (HIDBuffer[2] & 0x01); // Keep the rumble bit
923  HID_Command(HIDBuffer, 3);
924 }
925 
927  HIDBuffer[1] = 0x11;
928  HIDBuffer[2] &= ~(pgm_read_byte(&WII_LEDS[(uint8_t)a]));
929  HID_Command(HIDBuffer, 3);
930 }
931 
933  if(a == OFF)
934  setLedRaw(0);
935  else {
936  HIDBuffer[1] = 0x11;
937  HIDBuffer[2] |= pgm_read_byte(&WII_LEDS[(uint8_t)a]);
938  HID_Command(HIDBuffer, 3);
939  }
940 }
941 
943  HIDBuffer[1] = 0x11;
944  HIDBuffer[2] ^= pgm_read_byte(&WII_LEDS[(uint8_t)a]);
945  HID_Command(HIDBuffer, 3);
946 }
947 
949  HIDBuffer[1] = 0x11;
950  HIDBuffer[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit
951  if(wiimoteConnected)
952  HIDBuffer[2] |= 0x10; // If it's connected LED1 will light up
954  HIDBuffer[2] |= 0x20; // If it's connected LED2 will light up
956  HIDBuffer[2] |= 0x40; // If it's connected LED3 will light up
957 
958  HID_Command(HIDBuffer, 3);
959 }
960 
962  checkBatteryLevel = true; // This is needed so the library knows that the status response is a response to this function
963  statusRequest(); // This will update the battery level
964  return batteryLevel;
965 };
966 
967 void WII::setReportMode(bool continuous, uint8_t mode) {
968  uint8_t cmd_buf[4];
969  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
970  cmd_buf[1] = 0x12;
971  if(continuous)
972  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit
973  else
974  cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Keep the rumble bit
975  cmd_buf[3] = mode;
976  HID_Command(cmd_buf, 4);
977 }
978 
979 void WII::statusRequest() {
980  uint8_t cmd_buf[3];
981  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
982  cmd_buf[1] = 0x15;
983  cmd_buf[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit
984  HID_Command(cmd_buf, 3);
985 }
986 
987 /************************************************************/
988 /* Memmory Commands */
989 /************************************************************/
990 
991 void WII::writeData(uint32_t offset, uint8_t size, uint8_t* data) {
992  uint8_t cmd_buf[23];
993  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
994  cmd_buf[1] = 0x16; // Write data
995  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Write to memory, clear bit 2 to write to EEPROM
996  cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16);
997  cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8);
998  cmd_buf[5] = (uint8_t)(offset & 0xFF);
999  cmd_buf[6] = size;
1000  uint8_t i = 0;
1001  for(; i < size; i++)
1002  cmd_buf[7 + i] = data[i];
1003  for(; i < 16; i++) // Set the rest to zero
1004  cmd_buf[7 + i] = 0x00;
1005  HID_Command(cmd_buf, 23);
1006 }
1007 
1008 void WII::initExtension1() {
1009  uint8_t buf[1];
1010  buf[0] = 0x55;
1011  writeData(0xA400F0, 1, buf);
1012 }
1013 
1014 void WII::initExtension2() {
1015  uint8_t buf[1];
1016  buf[0] = 0x00;
1017  writeData(0xA400FB, 1, buf);
1018 }
1019 
1020 void WII::initMotionPlus() {
1021  uint8_t buf[1];
1022  buf[0] = 0x55;
1023  writeData(0xA600F0, 1, buf);
1024 }
1025 
1026 void WII::activateMotionPlus() {
1027  uint8_t buf[1];
1028  if(pBtd->wiiUProController) {
1029 #ifdef DEBUG_USB_HOST
1030  Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80);
1031 #endif
1032  buf[0] = 0x00; // It seems like you can send anything but 0x04, 0x05, and 0x07
1033  } else if(activateNunchuck) {
1034 #ifdef DEBUG_USB_HOST
1035  Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"), 0x80);
1036 #endif
1037  buf[0] = 0x05; // Activate nunchuck pass-through mode
1038  }//else if(classicControllerConnected && extensionConnected)
1039  //buf[0] = 0x07;
1040  else {
1041 #ifdef DEBUG_USB_HOST
1042  Notify(PSTR("\r\nActivating Motion Plus in normal mode"), 0x80);
1043 #endif
1044  buf[0] = 0x04; // Don't use any extension
1045  }
1046  writeData(0xA600FE, 1, buf);
1047 }
1048 
1049 void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) {
1050  uint8_t cmd_buf[8];
1051  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
1052  cmd_buf[1] = 0x17; // Read data
1053  if(EEPROM)
1054  cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Read from EEPROM
1055  else
1056  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory
1057  cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16);
1058  cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8);
1059  cmd_buf[5] = (uint8_t)(offset & 0xFF);
1060  cmd_buf[6] = (uint8_t)((size & 0xFF00) >> 8);
1061  cmd_buf[7] = (uint8_t)(size & 0xFF);
1062 
1063  HID_Command(cmd_buf, 8);
1064 }
1065 
1066 void WII::readExtensionType() {
1067  readData(0xA400FA, 6, false);
1068 }
1069 
1070 void WII::readCalData() {
1071  readData(0x0016, 8, true);
1072 }
1073 
1074 void WII::checkMotionPresent() {
1075  readData(0xA600FA, 6, false);
1076 }
1077 
1078 void WII::readWiiBalanceBoardCalibration() {
1079  readData(0xA40024, 24, false);
1080 }
1081 
1082 /************************************************************/
1083 /* WII Commands */
1084 /************************************************************/
1085 
1086 bool WII::getButtonPress(ButtonEnum b) { // Return true when a button is pressed
1088  return (ButtonState & pgm_read_dword(&WII_PROCONTROLLER_BUTTONS[(uint8_t)b]));
1089  else
1090  return (ButtonState & pgm_read_dword(&WII_BUTTONS[(uint8_t)b]));
1091 }
1092 
1093 bool WII::getButtonClick(ButtonEnum b) { // Only return true when a button is clicked
1094  uint32_t button;
1096  button = pgm_read_dword(&WII_PROCONTROLLER_BUTTONS[(uint8_t)b]);
1097  else
1098  button = pgm_read_dword(&WII_BUTTONS[(uint8_t)b]);
1099  bool click = (ButtonClickState & button);
1100  ButtonClickState &= ~button; // clear "click" event
1101  return click;
1102 }
1103 
1105  if(!nunchuckConnected)
1106  return 127; // Return center position
1107  else {
1108  uint8_t output = hatValues[(uint8_t)a];
1109  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
1110  return 127;
1111  else
1112  return output;
1113  }
1114 }
1115 
1118  return 2000;
1119  else {
1120  uint16_t output = hatValues[(uint8_t)a];
1121  if(output == 0x00) // The joystick will only read 0 when it is first initializing, so we will just return the center position
1122  return 2000;
1123  else
1124  return output;
1125  }
1126 }
1127 
1128 void WII::onInit() {
1129  if(pFuncOnInit)
1130  pFuncOnInit(); // Call the user function
1131  else
1132  setLedStatus();
1133 }
1134 
1135 /************************************************************/
1136 /* Wii Balance Board Commands */
1137 /************************************************************/
1138 
1140  // Use interpolating between two points - based on: https://github.com/skorokithakis/gr8w8upd8m8/blob/master/gr8w8upd8m8.py
1141  // wiiBalanceBoardCal[pos][0] is calibration values for 0 kg
1142  // wiiBalanceBoardCal[pos][1] is calibration values for 17 kg
1143  // wiiBalanceBoardCal[pos][2] is calibration values for 34 kg
1144  if(wiiBalanceBoardRaw[pos] < wiiBalanceBoardCal[0][pos])
1145  return 0.0f; // Below 0 kg
1146  else if(wiiBalanceBoardRaw[pos] < wiiBalanceBoardCal[1][pos]) // Between 0 and 17 kg
1147  return 17.0f * (float)(wiiBalanceBoardRaw[pos] - wiiBalanceBoardCal[0][pos]) / (float)(wiiBalanceBoardCal[1][pos] - wiiBalanceBoardCal[0][pos]);
1148  else // More than 17 kg
1149  return 17.0f + 17.0f * (float)(wiiBalanceBoardRaw[pos] - wiiBalanceBoardCal[1][pos]) / (float)(wiiBalanceBoardCal[2][pos] - wiiBalanceBoardCal[1][pos]);
1150 };
1151 
1154 };
1155 
1156 /************************************************************/
1157 /* The following functions are for the IR camera */
1158 /************************************************************/
1159 
1160 #ifdef WIICAMERA
1161 
1162 void WII::IRinitialize() { // Turns on and initialises the IR camera
1163 
1164  enableIRCamera1();
1165 #ifdef DEBUG_USB_HOST
1166  Notify(PSTR("\r\nEnable IR Camera1 Complete"), 0x80);
1167 #endif
1168  delay(80);
1169 
1170  enableIRCamera2();
1171 #ifdef DEBUG_USB_HOST
1172  Notify(PSTR("\r\nEnable IR Camera2 Complete"), 0x80);
1173 #endif
1174  delay(80);
1175 
1176  write0x08Value();
1177 #ifdef DEBUG_USB_HOST
1178  Notify(PSTR("\r\nWrote hex number 0x08"), 0x80);
1179 #endif
1180  delay(80);
1181 
1182  writeSensitivityBlock1();
1183 #ifdef DEBUG_USB_HOST
1184  Notify(PSTR("\r\nWrote Sensitivity Block 1"), 0x80);
1185 #endif
1186  delay(80);
1187 
1188  writeSensitivityBlock2();
1189 #ifdef DEBUG_USB_HOST
1190  Notify(PSTR("\r\nWrote Sensitivity Block 2"), 0x80);
1191 #endif
1192  delay(80);
1193 
1194  uint8_t mode_num = 0x03;
1195  setWiiModeNumber(mode_num); // Change input for whatever mode you want i.e. 0x01, 0x03, or 0x05
1196 #ifdef DEBUG_USB_HOST
1197  Notify(PSTR("\r\nSet Wii Mode Number To 0x"), 0x80);
1198  D_PrintHex<uint8_t > (mode_num, 0x80);
1199 #endif
1200  delay(80);
1201 
1202  write0x08Value();
1203 #ifdef DEBUG_USB_HOST
1204  Notify(PSTR("\r\nWrote Hex Number 0x08"), 0x80);
1205 #endif
1206  delay(80);
1207 
1208  setReportMode(false, 0x33);
1209  //setReportMode(false, 0x3f); // For full reporting mode, doesn't work yet
1210 #ifdef DEBUG_USB_HOST
1211  Notify(PSTR("\r\nSet Report Mode to 0x33"), 0x80);
1212 #endif
1213  delay(80);
1214 
1215  statusRequest(); // Used to update wiiState - call isIRCameraEnabled() afterwards to check if it actually worked
1216 #ifdef DEBUG_USB_HOST
1217  Notify(PSTR("\r\nIR Initialized"), 0x80);
1218 #endif
1219 }
1220 
1221 void WII::enableIRCamera1() {
1222  uint8_t cmd_buf[3];
1223  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
1224  cmd_buf[1] = 0x13; // Output report 13
1225  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
1226  HID_Command(cmd_buf, 3);
1227 }
1228 
1229 void WII::enableIRCamera2() {
1230  uint8_t cmd_buf[3];
1231  cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
1232  cmd_buf[1] = 0x1A; // Output report 1A
1233  cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
1234  HID_Command(cmd_buf, 3);
1235 }
1236 
1237 void WII::writeSensitivityBlock1() {
1238  uint8_t buf[9];
1239  buf[0] = 0x00;
1240  buf[1] = 0x00;
1241  buf[2] = 0x00;
1242  buf[3] = 0x00;
1243  buf[4] = 0x00;
1244  buf[5] = 0x00;
1245  buf[6] = 0x90;
1246  buf[7] = 0x00;
1247  buf[8] = 0x41;
1248 
1249  writeData(0xB00000, 9, buf);
1250 }
1251 
1252 void WII::writeSensitivityBlock2() {
1253  uint8_t buf[2];
1254  buf[0] = 0x40;
1255  buf[1] = 0x00;
1256 
1257  writeData(0xB0001A, 2, buf);
1258 }
1259 
1260 void WII::write0x08Value() {
1261  uint8_t cmd = 0x08;
1262  writeData(0xb00030, 1, &cmd);
1263 }
1264 
1265 void WII::setWiiModeNumber(uint8_t mode_number) { // mode_number in hex i.e. 0x03 for extended mode
1266  writeData(0xb00033, 1, &mode_number);
1267 }
1268 #endif
bool wiimoteConnected
Definition: Wii.h:191
#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE
Definition: BTD.h:137
bool incomingWii
Definition: BTD.h:471
void onInit()
Definition: Wii.cpp:1128
int16_t gyroPitchRaw
Definition: Wii.h:265
#define pgm_read_dword(addr)
#define wii_clear_flag(flag)
Definition: Wii.h:33
#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS
Definition: BTD.h:141
#define L2CAP_INTERRUPT_CONFIG_REQUEST
Definition: BTD.h:113
#define L2CAP_INTERRUPT_SETUP
Definition: BTD.h:111
uint16_t rollGyroScale
Definition: Wii.h:257
void l2cap_connection_response(uint16_t handle, uint8_t rxid, uint8_t *dcid, uint8_t *scid, uint8_t result)
Definition: BTD.cpp:1262
#define SUCCESSFUL
Definition: BTD.h:175
void l2cap_connection_request(uint16_t handle, uint8_t rxid, uint8_t *scid, uint16_t psm)
Definition: BTD.cpp:1249
#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST
Definition: BTD.h:140
void setLedToggle(LEDEnum a)
Definition: Wii.cpp:942
void setLedRaw(uint8_t value)
Definition: Wii.cpp:920
uint8_t getAnalogHat(HatEnum a)
Definition: Wii.cpp:1104
uint16_t yawGyroScale
Definition: Wii.h:258
Definition: Wii.h:40
Definition: BTD.h:198
#define WII_FLAG_NUNCHUCK_CONNECTED
Definition: Wii.h:28
void l2cap_disconnection_request(uint16_t handle, uint8_t rxid, uint8_t *dcid, uint8_t *scid)
Definition: BTD.cpp:1315
void ACLData(uint8_t *ACLData)
Definition: Wii.cpp:133
bool pairWithWii
Definition: BTD.h:473
uint8_t identifier
Definition: BTD.h:617
float pitchGyroSpeed
Definition: Wii.h:249
void setRumbleOn()
Definition: Wii.cpp:908
void Reset()
Definition: Wii.cpp:101
AnalogHatEnum
#define TURN_ON_LED
Definition: BTD.h:127
int16_t accZnunchuck
Definition: Wii.h:236
Definition: Wii.h:48
uint8_t getBatteryLevel()
Definition: Wii.cpp:961
void disconnect()
Definition: Wii.cpp:116
bool motionPlusInside
Definition: BTD.h:475
#define L2CAP_DONE
Definition: BTD.h:102
#define wii_set_flag(flag)
Definition: Wii.h:32
#define L2CAP_CONTROL_SUCCESS
Definition: BTD.h:107
#define L2CAP_WAIT
Definition: BTD.h:101
Definition: Wii.h:47
int16_t accXnunchuck
Definition: Wii.h:236
#define pgm_read_byte(addr)
#define L2CAP_CONTROL_CONFIG_REQUEST
Definition: BTD.h:106
int16_t gyroRollRaw
Definition: Wii.h:264
LEDEnum
void l2cap_disconnection_response(uint16_t handle, uint8_t rxid, uint8_t *dcid, uint8_t *scid)
Definition: BTD.cpp:1328
void IRinitialize()
Definition: Wii.cpp:1162
Definition: Wii.h:38
#define Notify(...)
Definition: message.h:44
void setRumbleToggle()
Definition: Wii.cpp:914
int16_t gyroPitchZero
Definition: Wii.h:272
uint16_t pitchGyroScale
Definition: Wii.h:256
#define L2CAP_CONTROL_CONNECT_REQUEST
Definition: BTD.h:105
#define HID_CTRL_PSM
Definition: BTD.h:180
WII(BTD *p, bool pair=false)
Definition: Wii.cpp:85
#define WII_CHECK_EXTENSION_STATE
Definition: BTD.h:130
float rollGyroSpeed
Definition: Wii.h:250
bool connectToWii
Definition: BTD.h:467
bool wiiUProController
Definition: BTD.h:477
uint16_t hci_handle
Definition: BTD.h:451
void hci_disconnect(uint16_t handle)
Definition: BTD.cpp:1173
void setLedStatus()
Definition: Wii.cpp:948
#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE
Definition: BTD.h:143
BalanceBoardEnum
Definition: Wii.h:44
#define WII_FLAG_MOTION_PLUS_CONNECTED
Definition: Wii.h:27
const uint8_t WII_LEDS[]
Definition: Wii.cpp:25
ButtonEnum
bool getButtonClick(ButtonEnum b)
Definition: Wii.cpp:1093
int16_t accYwiimote
Definition: Wii.h:230
bool wiiUProControllerConnected
Definition: Wii.h:202
void(* pFuncOnInit)(void)
Definition: BTD.h:605
#define WII_FLAG_CALIBRATE_BALANCE_BOARD
Definition: Wii.h:29
#define l2cap_check_flag(flag)
Definition: BTD.h:158
void setRumbleOff()
Definition: Wii.cpp:902
bool wiiBalanceBoardConnected
Definition: Wii.h:204
#define L2CAP_CMD_CONFIG_REQUEST
Definition: BTD.h:166
#define L2CAP_FLAG_CONTROL_CONNECTED
Definition: BTD.h:136
#define PSTR(str)
#define L2CAP_CMD_DISCONNECT_REQUEST
Definition: BTD.h:168
float gyroRoll
Definition: Wii.h:243
void setAllOff()
Definition: Wii.cpp:896
#define L2CAP_CONTROL_DISCONNECT
Definition: BTD.h:108
const uint32_t WII_BUTTONS[]
Definition: Wii.cpp:40
#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST
Definition: BTD.h:134
void setLedOff()
Definition: Wii.h:152
float getWiimoteRoll()
Definition: Wii.h:216
#define WII_CHECK_MOTION_PLUS_STATE
Definition: BTD.h:129
BTD * pBtd
Definition: BTD.h:608
#define HID_INTR_PSM
Definition: BTD.h:181
bool nunchuckConnected
Definition: Wii.h:198
bool l2capConnectionClaimed
Definition: BTD.h:437
float gyroPitch
Definition: Wii.h:241
Definition: Wii.h:45
int16_t gyroYawRaw
Definition: Wii.h:263
float gyroYaw
Definition: Wii.h:245
bool isIRCameraEnabled()
Definition: Wii.h:408
const uint32_t WII_PROCONTROLLER_BUTTONS[]
Definition: Wii.cpp:59
float yawGyroSpeed
Definition: Wii.h:251
#define L2CAP_CMD_DISCONNECT_RESPONSE
Definition: BTD.h:169
#define L2CAP_CMD_CONNECTION_RESPONSE
Definition: BTD.h:165
#define L2CAP_CMD_CONFIG_RESPONSE
Definition: BTD.h:167
void pair(void)
Definition: Wii.h:89
uint16_t hci_handle
Definition: BTD.h:611
void setLedOn(LEDEnum a)
Definition: Wii.cpp:932
HatEnum
Definition: Wii.h:36
int16_t accYnunchuck
Definition: Wii.h:236
int16_t gyroYawZero
Definition: Wii.h:270
uint32_t l2cap_event_flag
Definition: BTD.h:614
#define wii_check_flag(flag)
Definition: Wii.h:31
Definition: Wii.h:46
#define WII_INIT_MOTION_PLUS_STATE
Definition: BTD.h:131
void L2CAP_Command(uint16_t handle, uint8_t *data, uint8_t nbytes, uint8_t channelLow=0x01, uint8_t channelHigh=0x00)
Definition: BTD.cpp:1221
void l2cap_config_response(uint16_t handle, uint8_t rxid, uint8_t *scid)
Definition: BTD.cpp:1296
#define PENDING
Definition: BTD.h:174
#define L2CAP_FLAG_INTERRUPT_CONNECTED
Definition: BTD.h:142
#define l2cap_set_flag(flag)
Definition: BTD.h:159
void l2cap_config_request(uint16_t handle, uint8_t rxid, uint8_t *dcid)
Definition: BTD.cpp:1279
bool getButtonPress(ButtonEnum b)
Definition: Wii.cpp:1086
#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS
Definition: BTD.h:135
#define L2CAP_CMD_CONNECTION_REQUEST
Definition: BTD.h:164
float getWiimotePitch()
Definition: Wii.h:212
bool motionPlusConnected
Definition: Wii.h:200
int16_t gyroRollZero
Definition: Wii.h:271
float getTotalWeight()
Definition: Wii.cpp:1152
bool checkHciHandle(uint8_t *buf, uint16_t handle)
Definition: BTD.h:600
#define L2CAP_INTERRUPT_CONNECT_REQUEST
Definition: BTD.h:112
void Run()
Definition: Wii.cpp:718
#define L2CAP_INTERRUPT_DISCONNECT
Definition: BTD.h:114
int16_t accXwiimote
Definition: Wii.h:230
float getWeight(BalanceBoardEnum pos)
Definition: Wii.cpp:1139
int16_t accZwiimote
Definition: Wii.h:230
#define L2CAP_CMD_COMMAND_REJECT
Definition: BTD.h:163