Zoltan Hudak / UsbHostMAX3421E

MAX3421E-based USB Host Shield Library

Dependents:   UsbHostMAX3421E_Hello

Wii.cpp@1:2263e77400e9, 2020-07-13 (annotated)

Committer:
hudakz
Date:
Mon Jul 13 07:03:06 2020 +0000
Revision:
1:2263e77400e9
Parent:
0:84353c479782 
MAX3421E-based USB Host Shield Library

Who changed what in which revision?

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