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