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