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