1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
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
13 Kristian Lauszus, TKJ Electronics
14 Web : http://www.tkjelectronics.com
15 e-mail : kristianl@tkjelectronics.com
17 IR camera support added by Allan Glover (adglover9.81@gmail.com) and Kristian Lauszus
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
25 const uint8_t WII_LEDS
[] PROGMEM
= {
40 const uint32_t WII_BUTTONS
[] PROGMEM
= {
59 const uint32_t WII_PROCONTROLLER_BUTTONS
[] PROGMEM
= {
85 WII::WII(BTD
*p
, bool pair
) :
86 BluetoothService(p
) // Pointer to USB class instance - mandatory
88 pBtd
->pairWithWii
= pair
;
90 HIDBuffer
[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
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;
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
;
116 void WII::disconnect() { // Use this void to disconnect any of the controllers
117 if(!motionPlusInside
) { // The old Wiimote needs a delay after the first command or it will automatically reconnect
118 if(motionPlusConnected
) {
119 #ifdef DEBUG_USB_HOST
120 Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80);
122 initExtension1(); // This will disable the Motion Plus extension
124 timer
= millis() + 1000; // We have to wait for the message before the rest of the channels can be deactivated
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
);
130 l2cap_state
= L2CAP_INTERRUPT_DISCONNECT
;
133 void WII::ACLData(uint8_t* l2capinbuf
) {
134 if(!pBtd
->l2capConnectionClaimed
&& pBtd
->incomingWii
&& !wiimoteConnected
&& !activeConnection
) {
135 if(l2capinbuf
[8] == L2CAP_CMD_CONNECTION_REQUEST
) {
136 if((l2capinbuf
[12] | (l2capinbuf
[13] << 8)) == HID_CTRL_PSM
) {
137 motionPlusInside
= pBtd
->motionPlusInside
;
138 pBtd
->incomingWii
= false;
139 pBtd
->l2capConnectionClaimed
= true; // Claim that the incoming connection belongs to this service
140 activeConnection
= true;
141 hci_handle
= pBtd
->hci_handle
; // Store the HCI Handle for the connection
142 l2cap_state
= L2CAP_WAIT
;
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);
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
);
180 } else if(l2capinbuf
[8] == L2CAP_CMD_CONNECTION_REQUEST
) {
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);
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
);
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
);
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
);
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);
229 identifier
= l2capinbuf
[9];
230 pBtd
->l2cap_disconnection_response(hci_handle
, identifier
, control_dcid
, control_scid
);
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);
236 identifier
= l2capinbuf
[9];
237 pBtd
->l2cap_disconnection_response(hci_handle
, identifier
, interrupt_dcid
, interrupt_scid
);
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
);
253 identifier
= l2capinbuf
[9];
254 Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
255 D_PrintHex
<uint8_t > (l2capinbuf
[8], 0x80);
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));
278 Notify(PSTR("ButtonState: "), 0x80);
279 D_PrintHex
<uint32_t > (ButtonState
, 0x80);
280 Notify(PSTR("\r\n"), 0x80);
282 if(ButtonState
!= OldButtonState
) {
283 ButtonClickState
= ButtonState
& ~OldButtonState
; // Update click state variable
284 OldButtonState
= ButtonState
;
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;
292 switch(l2capinbuf
[9]) {
293 case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV
295 Notify(PSTR("\r\nStatus report was received"), 0x80);
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
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);
306 unknownExtensionConnected
= true;
308 if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
310 setReportMode(false, 0x35); // Also read the extension
312 #ifdef DEBUG_USB_HOST
313 Notify(PSTR("\r\nExtension disconnected"), 0x80);
315 if(motionPlusConnected
) {
316 #ifdef DEBUG_USB_HOST
317 Notify(PSTR(" - from Motion Plus"), 0x80);
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);
327 nunchuckConnected
= false; // It must be the Nunchuck controller then
328 wii_clear_flag(WII_FLAG_NUNCHUCK_CONNECTED
);
330 setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
332 setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
337 Notify(PSTR("\r\nChecking battery level"), 0x80);
339 checkBatteryLevel
= false; // Check for extensions by default
341 #ifdef DEBUG_USB_HOST
342 if(l2capinbuf
[12] & 0x01)
343 Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80);
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);
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);
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);
365 motionPlusConnected
= true;
367 if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
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);
374 activateNunchuck
= false;
375 motionPlusConnected
= true;
376 nunchuckConnected
= true;
378 if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
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);
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);
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);
396 setReportMode(false, 0x32); // Read the Wii Balance Board extension
397 wii_set_flag(WII_FLAG_CALIBRATE_BALANCE_BOARD
);
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;
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);
411 wii_clear_flag(WII_FLAG_CALIBRATE_BALANCE_BOARD
);
412 wiiBalanceBoardConnected
= true;
414 #ifdef DEBUG_USB_HOST
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);
429 Notify(PSTR("\r\nReport Error: "), 0x80);
430 D_PrintHex
<uint8_t > (l2capinbuf
[13], 0x80);
431 D_PrintHex
<uint8_t > (l2capinbuf
[14], 0x80);
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);
443 case 0x30: // Core buttons - (a1) 30 BB BB
445 case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA
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
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
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
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);
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);
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);
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
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
481 /**** for reading in full mode: DOES NOT WORK YET ****/
482 /* When it works it will also have intensity and bounding box data */
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);
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);
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
);
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
);
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;
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());
521 gyroYaw
+= (yawGyroSpeed
* ((double)(micros() - timer
) / 1000000));
522 gyroRoll
+= (rollGyroSpeed
* ((double)(micros() - timer
) / 1000000));
523 gyroPitch
+= (pitchGyroSpeed
* ((double)(micros() - timer
) / 1000000));
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);
535 Notify(PSTR("\twiimoteRoll: "), 0x80);
536 Notify(wiimoteRoll, 0x80);
537 Notify(PSTR("\twiimotePitch: "), 0x80);
538 Notify(wiimotePitch, 0x80);
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);
545 gyroYawZero
= (l2capinbuf
[15] | ((l2capinbuf
[18] & 0xFC) << 6));
546 gyroRollZero
= (l2capinbuf
[16] | ((l2capinbuf
[19] & 0xFC) << 6));
547 gyroPitchZero
= (l2capinbuf
[17] | ((l2capinbuf
[20] & 0xFC) << 6));
549 rollGyroScale
= 500; // You might need to adjust these
550 pitchGyroScale
= 400;
557 motionValuesReset
= true;
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;
569 //else if(classicControllerConnected) { }
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);
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);
586 nunchuckConnected
= false; // There is no extension connected to the Motion Plus if this report is sent
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);
604 #ifdef DEBUG_USB_HOST
606 Notify(PSTR("\r\nUnknown Report type: "), 0x80);
607 D_PrintHex
<uint8_t > (l2capinbuf
[9], 0x80);
617 void 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);
625 l2cap_state
= L2CAP_INTERRUPT_SETUP
;
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);
634 pBtd
->l2cap_connection_response(hci_handle
, identifier
, interrupt_dcid
, interrupt_scid
, PENDING
);
636 pBtd
->l2cap_connection_response(hci_handle
, identifier
, interrupt_dcid
, interrupt_scid
, SUCCESSFUL
);
639 pBtd
->l2cap_config_request(hci_handle
, identifier
, interrupt_scid
);
641 l2cap_state
= L2CAP_INTERRUPT_CONFIG_REQUEST
;
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);
652 pBtd
->l2cap_config_request(hci_handle
, identifier
, control_scid
);
653 l2cap_state
= L2CAP_CONTROL_CONFIG_REQUEST
;
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);
663 pBtd
->l2cap_connection_request(hci_handle
, identifier
, interrupt_dcid
, HID_INTR_PSM
);
664 l2cap_state
= L2CAP_INTERRUPT_CONNECT_REQUEST
;
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);
674 pBtd
->l2cap_config_request(hci_handle
, identifier
, interrupt_scid
);
675 l2cap_state
= L2CAP_INTERRUPT_CONFIG_REQUEST
;
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);
684 pBtd
->connectToWii
= false;
685 pBtd
->pairWithWii
= false;
687 l2cap_state
= WII_CHECK_MOTION_PLUS_STATE
;
691 /* The next states are in run() */
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);
699 pBtd
->l2cap_disconnection_request(hci_handle
, identifier
, control_scid
, control_dcid
);
700 l2cap_state
= L2CAP_CONTROL_DISCONNECT
;
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);
709 pBtd
->hci_disconnect(hci_handle
);
710 hci_handle
= -1; // Reset handle
711 l2cap_event_flag
= 0; // Reset flags
712 l2cap_state
= L2CAP_WAIT
;
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
722 switch(l2cap_state
) {
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);
731 hci_handle
= pBtd
->hci_handle
; // Store the HCI Handle for the connection
732 l2cap_event_flag
= 0; // Reset flags
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);
740 pBtd
->l2cap_connection_response(hci_handle
, identifier
, control_dcid
, control_scid
, PENDING
);
742 pBtd
->l2cap_connection_response(hci_handle
, identifier
, control_dcid
, control_scid
, SUCCESSFUL
);
745 pBtd
->l2cap_config_request(hci_handle
, identifier
, control_scid
);
746 l2cap_state
= L2CAP_CONTROL_SUCCESS
;
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);
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
)) {
760 l2cap_state
= WII_INIT_MOTION_PLUS_STATE
;
763 if(unknownExtensionConnected
) {
764 #ifdef DEBUG_USB_HOST
765 Notify(PSTR("\r\nA extension is also connected"), 0x80);
767 activateNunchuck
= true; // For we will just set this to true as this the only extension supported so far
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);
775 l2cap_state
= WII_CHECK_EXTENSION_STATE
;
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);
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
792 } else if(stateCounter
== 200)
794 else if(stateCounter
== 300) {
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);
802 readWiiBalanceBoardCalibration();
805 } else if(stateCounter
== 500) {
807 l2cap_state
= TURN_ON_LED
;
811 case WII_INIT_MOTION_PLUS_STATE
:
813 if(stateCounter
== 1)
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) {
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
;
827 if(wii_check_flag(WII_FLAG_NUNCHUCK_CONNECTED
))
828 nunchuckConnected
= true;
829 wiimoteConnected
= true;
831 l2cap_state
= 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);
840 stateCounter
++; // We will use this counter as there has to be a short delay between the commands
841 if(stateCounter
== 50)
843 else if(stateCounter
== 100)
845 else if(stateCounter
== 150)
846 if((extensionConnected
&& motionPlusConnected
) || (unknownExtensionConnected
&& !motionPlusConnected
))
849 stateCounter
= 299; // There is no extension connected
850 else if(stateCounter
== 200)
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);
857 activateNunchuck
= true;
858 nunchuckConnected
= true;
860 if(!motionPlusConnected
)
862 } else if(stateCounter
== 300) {
863 if(motionPlusConnected
) {
864 #ifdef DEBUG_USB_HOST
865 Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80);
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) {
877 unknownExtensionConnected
= false;
885 /************************************************************/
887 /************************************************************/
889 void WII::HID_Command(uint8_t* data
, uint8_t nbytes
) {
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
893 pBtd
->L2CAP_Command(hci_handle
, data
, nbytes
, control_scid
[0], control_scid
[1]);
896 void WII::setAllOff() {
899 HID_Command(HIDBuffer
, 3);
902 void WII::setRumbleOff() {
904 HIDBuffer
[2] &= ~0x01; // Bit 0 control the rumble
905 HID_Command(HIDBuffer
, 3);
908 void WII::setRumbleOn() {
910 HIDBuffer
[2] |= 0x01; // Bit 0 control the rumble
911 HID_Command(HIDBuffer
, 3);
914 void WII::setRumbleToggle() {
916 HIDBuffer
[2] ^= 0x01; // Bit 0 control the rumble
917 HID_Command(HIDBuffer
, 3);
920 void WII::setLedRaw(uint8_t value
) {
922 HIDBuffer
[2] = value
| (HIDBuffer
[2] & 0x01); // Keep the rumble bit
923 HID_Command(HIDBuffer
, 3);
926 void WII::setLedOff(LEDEnum a
) {
928 HIDBuffer
[2] &= ~(pgm_read_byte(&WII_LEDS
[(uint8_t)a
]));
929 HID_Command(HIDBuffer
, 3);
932 void WII::setLedOn(LEDEnum a
) {
937 HIDBuffer
[2] |= pgm_read_byte(&WII_LEDS
[(uint8_t)a
]);
938 HID_Command(HIDBuffer
, 3);
942 void WII::setLedToggle(LEDEnum a
) {
944 HIDBuffer
[2] ^= pgm_read_byte(&WII_LEDS
[(uint8_t)a
]);
945 HID_Command(HIDBuffer
, 3);
948 void WII::setLedStatus() {
950 HIDBuffer
[2] = (HIDBuffer
[2] & 0x01); // Keep the rumble bit
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
958 HID_Command(HIDBuffer
, 3);
961 uint8_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
967 void WII::setReportMode(bool continuous
, uint8_t mode
) {
969 cmd_buf
[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
972 cmd_buf
[2] = 0x04 | (HIDBuffer
[2] & 0x01); // Keep the rumble bit
974 cmd_buf
[2] = 0x00 | (HIDBuffer
[2] & 0x01); // Keep the rumble bit
976 HID_Command(cmd_buf
, 4);
979 void WII::statusRequest() {
981 cmd_buf
[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
983 cmd_buf
[2] = (HIDBuffer
[2] & 0x01); // Keep the rumble bit
984 HID_Command(cmd_buf
, 3);
987 /************************************************************/
988 /* Memmory Commands */
989 /************************************************************/
991 void WII::writeData(uint32_t offset
, uint8_t size
, uint8_t* data
) {
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);
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);
1008 void WII::initExtension1() {
1011 writeData(0xA400F0, 1, buf
);
1014 void WII::initExtension2() {
1017 writeData(0xA400FB, 1, buf
);
1020 void WII::initMotionPlus() {
1023 writeData(0xA600F0, 1, buf
);
1026 void WII::activateMotionPlus() {
1028 if(pBtd
->wiiUProController
) {
1029 #ifdef DEBUG_USB_HOST
1030 Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80);
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);
1037 buf
[0] = 0x05; // Activate nunchuck pass-through mode
1038 }//else if(classicControllerConnected && extensionConnected)
1041 #ifdef DEBUG_USB_HOST
1042 Notify(PSTR("\r\nActivating Motion Plus in normal mode"), 0x80);
1044 buf
[0] = 0x04; // Don't use any extension
1046 writeData(0xA600FE, 1, buf
);
1049 void WII::readData(uint32_t offset
, uint16_t size
, bool EEPROM
) {
1051 cmd_buf
[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
1052 cmd_buf
[1] = 0x17; // Read data
1054 cmd_buf
[2] = 0x00 | (HIDBuffer
[2] & 0x01); // Read from EEPROM
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);
1063 HID_Command(cmd_buf
, 8);
1066 void WII::readExtensionType() {
1067 readData(0xA400FA, 6, false);
1070 void WII::readCalData() {
1071 readData(0x0016, 8, true);
1074 void WII::checkMotionPresent() {
1075 readData(0xA600FA, 6, false);
1078 void WII::readWiiBalanceBoardCalibration() {
1079 readData(0xA40024, 24, false);
1082 /************************************************************/
1084 /************************************************************/
1086 bool 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
]));
1090 return (ButtonState
& pgm_read_dword(&WII_BUTTONS
[(uint8_t)b
]));
1093 bool WII::getButtonClick(ButtonEnum b
) { // Only return true when a button is clicked
1095 if(wiiUProControllerConnected
)
1096 button
= pgm_read_dword(&WII_PROCONTROLLER_BUTTONS
[(uint8_t)b
]);
1098 button
= pgm_read_dword(&WII_BUTTONS
[(uint8_t)b
]);
1099 bool click
= (ButtonClickState
& button
);
1100 ButtonClickState
&= ~button
; // clear "click" event
1104 uint8_t WII::getAnalogHat(HatEnum a
) {
1105 if(!nunchuckConnected
)
1106 return 127; // Return center position
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
1116 uint16_t WII::getAnalogHat(AnalogHatEnum a
) {
1117 if(!wiiUProControllerConnected
)
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
1128 void WII::onInit() {
1130 pFuncOnInit(); // Call the user function
1135 /************************************************************/
1136 /* Wii Balance Board Commands */
1137 /************************************************************/
1139 float 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
]);
1152 float WII::getTotalWeight() {
1153 return getWeight(TopRight
) + getWeight(BotRight
) + getWeight(TopLeft
) + getWeight(BotLeft
);
1156 /************************************************************/
1157 /* The following functions are for the IR camera */
1158 /************************************************************/
1162 void WII::IRinitialize() { // Turns on and initialises the IR camera
1165 #ifdef DEBUG_USB_HOST
1166 Notify(PSTR("\r\nEnable IR Camera1 Complete"), 0x80);
1171 #ifdef DEBUG_USB_HOST
1172 Notify(PSTR("\r\nEnable IR Camera2 Complete"), 0x80);
1177 #ifdef DEBUG_USB_HOST
1178 Notify(PSTR("\r\nWrote hex number 0x08"), 0x80);
1182 writeSensitivityBlock1();
1183 #ifdef DEBUG_USB_HOST
1184 Notify(PSTR("\r\nWrote Sensitivity Block 1"), 0x80);
1188 writeSensitivityBlock2();
1189 #ifdef DEBUG_USB_HOST
1190 Notify(PSTR("\r\nWrote Sensitivity Block 2"), 0x80);
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);
1203 #ifdef DEBUG_USB_HOST
1204 Notify(PSTR("\r\nWrote Hex Number 0x08"), 0x80);
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);
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);
1221 void WII::enableIRCamera1() {
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);
1229 void WII::enableIRCamera2() {
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);
1237 void WII::writeSensitivityBlock1() {
1249 writeData(0xB00000, 9, buf
);
1252 void WII::writeSensitivityBlock2() {
1257 writeData(0xB0001A, 2, buf
);
1260 void WII::write0x08Value() {
1262 writeData(0xb00030, 1, &cmd
);
1265 void WII::setWiiModeNumber(uint8_t mode_number
) { // mode_number in hex i.e. 0x03 for extended mode
1266 writeData(0xb00033, 1, &mode_number
);