mirror of
https://github.com/felis/USB_Host_Shield_2.0.git
synced 2024-03-22 11:31:26 +01:00
Fixed indent
This commit is contained in:
Kristian Sloth Lauszus
parent
d0ec18ab44
commit
84a66bb534
1 changed files with 319 additions and 319 deletions
638
Wii.cpp
638
Wii.cpp
|
|
@ -257,329 +257,329 @@ void WII::ACLData(uint8_t* l2capinbuf) {
|
||||||
#endif
|
#endif
|
||||||
} else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
|
} else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
|
||||||
//Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
|
//Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
|
||||||
if (l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
|
if (l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
|
||||||
if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
|
if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
|
||||||
if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
|
if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
|
||||||
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
|
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
|
||||||
else if (wiiUProControllerConnected)
|
else if (wiiUProControllerConnected)
|
||||||
ButtonState = (uint32_t)(((~l2capinbuf[23]) & 0xFE) | ((uint16_t)(~l2capinbuf[24]) << 8) | ((uint32_t)((~l2capinbuf[25]) & 0x03) << 16));
|
ButtonState = (uint32_t)(((~l2capinbuf[23]) & 0xFE) | ((uint16_t)(~l2capinbuf[24]) << 8) | ((uint32_t)((~l2capinbuf[25]) & 0x03) << 16));
|
||||||
else if (motionPlusConnected) {
|
else if (motionPlusConnected) {
|
||||||
if (l2capinbuf[20] & 0x02) // Only update the wiimote buttons, since the extension bytes are from the Motion Plus
|
if (l2capinbuf[20] & 0x02) // Only update the wiimote buttons, since the extension bytes are from the Motion Plus
|
||||||
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000)));
|
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000)));
|
||||||
else if (nunchuckConnected) // Update if it's a report from the Nunchuck
|
else if (nunchuckConnected) // Update if it's a report from the Nunchuck
|
||||||
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x0C) << 14));
|
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x0C) << 14));
|
||||||
//else if(classicControllerConnected) // Update if it's a report from the Classic Controller
|
//else if(classicControllerConnected) // Update if it's a report from the Classic Controller
|
||||||
} else if (nunchuckConnected) // The Nunchuck is directly connected
|
} else if (nunchuckConnected) // The Nunchuck is directly connected
|
||||||
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x03) << 16));
|
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x03) << 16));
|
||||||
//else if(classicControllerConnected) // The Classic Controller is directly connected
|
//else if(classicControllerConnected) // The Classic Controller is directly connected
|
||||||
else if (!unknownExtensionConnected)
|
else if (!unknownExtensionConnected)
|
||||||
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
|
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
|
||||||
#ifdef PRINTREPORT
|
#ifdef PRINTREPORT
|
||||||
Notify(PSTR("ButtonState: "), 0x80);
|
Notify(PSTR("ButtonState: "), 0x80);
|
||||||
D_PrintHex<uint32_t > (ButtonState, 0x80);
|
D_PrintHex<uint32_t > (ButtonState, 0x80);
|
||||||
Notify(PSTR("\r\n"), 0x80);
|
Notify(PSTR("\r\n"), 0x80);
|
||||||
#endif
|
|
||||||
if (ButtonState != OldButtonState) {
|
|
||||||
ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
|
|
||||||
OldButtonState = ButtonState;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer
|
|
||||||
accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5)) - 500;
|
|
||||||
accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500;
|
|
||||||
accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500;
|
|
||||||
wiimotePitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
|
|
||||||
wiimoteRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
|
|
||||||
}
|
|
||||||
switch (l2capinbuf[9]) {
|
|
||||||
case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV
|
|
||||||
#ifdef EXTRADEBUG
|
|
||||||
Notify(PSTR("\r\nStatus report was received"), 0x80);
|
|
||||||
#endif
|
|
||||||
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)
|
|
||||||
batteryLevel = l2capinbuf[15]; // Update battery level
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
if (l2capinbuf[12] & 0x01)
|
|
||||||
Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80);
|
|
||||||
#endif
|
|
||||||
if (checkExtension) { // If this is false it means that the user must have called getBatteryLevel()
|
|
||||||
if (l2capinbuf[12] & 0x02) { // Check if a extension is connected
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
if (!unknownExtensionConnected)
|
|
||||||
Notify(PSTR("\r\nExtension connected"), 0x80);
|
|
||||||
#endif
|
|
||||||
unknownExtensionConnected = true;
|
|
||||||
#ifdef WIICAMERA
|
|
||||||
if (!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
|
|
||||||
#endif
|
|
||||||
setReportMode(false, 0x35); // Also read the extension
|
|
||||||
} else {
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nExtension disconnected"), 0x80);
|
|
||||||
#endif
|
|
||||||
if (motionPlusConnected) {
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR(" - from Motion Plus"), 0x80);
|
|
||||||
#endif
|
|
||||||
l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
|
|
||||||
if (!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false
|
|
||||||
nunchuckConnected = false;
|
|
||||||
//else if(classicControllerConnected)
|
|
||||||
} else if (nunchuckConnected) {
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR(" - Nunchuck"), 0x80);
|
|
||||||
#endif
|
|
||||||
nunchuckConnected = false; // It must be the Nunchuck controller then
|
|
||||||
l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
|
|
||||||
onInit();
|
|
||||||
setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
|
|
||||||
} else
|
|
||||||
setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
|
|
||||||
}
|
|
||||||
} else
|
|
||||||
checkExtension = true; // Check for extensions by default
|
|
||||||
break;
|
|
||||||
case 0x21: // Read Memory Data
|
|
||||||
if ((l2capinbuf[12] & 0x0F) == 0) { // No error
|
|
||||||
// See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
|
|
||||||
if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nNunchuck connected"), 0x80);
|
|
||||||
#endif
|
|
||||||
l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED;
|
|
||||||
} else if (l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) {
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nMotion Plus connected"), 0x80);
|
|
||||||
#endif
|
|
||||||
l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED;
|
|
||||||
} else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) {
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80);
|
|
||||||
#endif
|
|
||||||
motionPlusConnected = true;
|
|
||||||
setReportMode(false, 0x35); // Also read the extension
|
|
||||||
} else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80);
|
|
||||||
#endif
|
|
||||||
activateNunchuck = false;
|
|
||||||
motionPlusConnected = true;
|
|
||||||
nunchuckConnected = true;
|
|
||||||
setReportMode(false, 0x35); // Also read the extension
|
|
||||||
} 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) {
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80);
|
|
||||||
Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80);
|
|
||||||
#endif
|
|
||||||
stateCounter = 300; // Skip the rest in "L2CAP_CHECK_MOTION_PLUS_STATE"
|
|
||||||
} else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) {
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80);
|
|
||||||
#endif
|
|
||||||
wiiUProControllerConnected = true;
|
|
||||||
}
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
else {
|
|
||||||
Notify(PSTR("\r\nUnknown Device: "), 0x80);
|
|
||||||
D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
|
|
||||||
D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
|
|
||||||
Notify(PSTR("\r\nData: "), 0x80);
|
|
||||||
for (uint8_t i = 0; i < ((l2capinbuf[12] >> 4) + 1); i++) { // bit 4-7 is the length-1
|
|
||||||
D_PrintHex<uint8_t > (l2capinbuf[15 + i], 0x80);
|
|
||||||
Notify(PSTR(" "), 0x80);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
#ifdef EXTRADEBUG
|
|
||||||
else {
|
|
||||||
Notify(PSTR("\r\nReport Error: "), 0x80);
|
|
||||||
D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
|
|
||||||
D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
break;
|
|
||||||
case 0x22: // Acknowledge output report, return function result
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
if (l2capinbuf[13] != 0x00) { // Check if there is an error
|
|
||||||
Notify(PSTR("\r\nCommand failed: "), 0x80);
|
|
||||||
D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
break;
|
|
||||||
case 0x30: // Core buttons - (a1) 30 BB BB
|
|
||||||
break;
|
|
||||||
case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA
|
|
||||||
pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected
|
|
||||||
roll = wiimoteRoll;
|
|
||||||
break;
|
|
||||||
case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
|
|
||||||
break;
|
|
||||||
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
|
|
||||||
pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus data available
|
|
||||||
roll = wiimoteRoll;
|
|
||||||
#ifdef WIICAMERA
|
|
||||||
// Read the IR data
|
|
||||||
IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position
|
|
||||||
IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position
|
|
||||||
IR_object_s1 = (l2capinbuf[17] & 0x0F); // size value, 0-15
|
|
||||||
|
|
||||||
IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4));
|
|
||||||
IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2));
|
|
||||||
IR_object_s2 = (l2capinbuf[20] & 0x0F);
|
|
||||||
|
|
||||||
IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4));
|
|
||||||
IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2));
|
|
||||||
IR_object_s3 = (l2capinbuf[23] & 0x0F);
|
|
||||||
|
|
||||||
IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4));
|
|
||||||
IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2));
|
|
||||||
IR_object_s4 = (l2capinbuf[26] & 0x0F);
|
|
||||||
#endif
|
|
||||||
break;
|
|
||||||
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
|
|
||||||
break;
|
|
||||||
/* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */
|
|
||||||
case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes
|
|
||||||
// (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
|
|
||||||
// corresponds to output report mode 0x3e
|
|
||||||
|
|
||||||
/**** for reading in full mode: DOES NOT WORK YET ****/
|
|
||||||
/* When it works it will also have intensity and bounding box data */
|
|
||||||
/*
|
|
||||||
IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
|
|
||||||
IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
|
|
||||||
IR_object_s1 = (l2capinbuf[15] & 0x0F);
|
|
||||||
*/
|
|
||||||
break;
|
|
||||||
case 0x3F:
|
|
||||||
/*
|
|
||||||
IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
|
|
||||||
IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
|
|
||||||
IR_object_s1 = (l2capinbuf[15] & 0x0F);
|
|
||||||
*/
|
|
||||||
break;
|
|
||||||
case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes
|
|
||||||
// (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
|
|
||||||
if (motionPlusConnected) {
|
|
||||||
if (l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
|
|
||||||
if (motionValuesReset) { // We will only use the values when the gyro value has been set
|
|
||||||
gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)) - gyroYawZero);
|
|
||||||
gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero);
|
|
||||||
gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero);
|
|
||||||
|
|
||||||
yawGyroSpeed = (double)gyroYawRaw / ((double)gyroYawZero / yawGyroScale);
|
|
||||||
rollGyroSpeed = -(double)gyroRollRaw / ((double)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values
|
|
||||||
pitchGyroSpeed = (double)gyroPitchRaw / ((double)gyroPitchZero / pitchGyroScale);
|
|
||||||
|
|
||||||
/* The onboard gyro has two ranges for slow and fast mode */
|
|
||||||
if (!(l2capinbuf[18] & 0x02)) // Check if fast more is used
|
|
||||||
yawGyroSpeed *= 4.545;
|
|
||||||
if (!(l2capinbuf[18] & 0x01)) // Check if fast more is used
|
|
||||||
pitchGyroSpeed *= 4.545;
|
|
||||||
if (!(l2capinbuf[19] & 0x02)) // Check if fast more is used
|
|
||||||
rollGyroSpeed *= 4.545;
|
|
||||||
|
|
||||||
pitch = (0.93 * (pitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimotePitch); // Use a complimentary filter to calculate the angle
|
|
||||||
roll = (0.93 * (roll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimoteRoll);
|
|
||||||
|
|
||||||
gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000));
|
|
||||||
gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000));
|
|
||||||
gyroPitch += (pitchGyroSpeed * ((double)(micros() - timer) / 1000000));
|
|
||||||
timer = micros();
|
|
||||||
/*
|
|
||||||
// Uncomment these lines to tune the gyro scale variabels
|
|
||||||
Notify(PSTR("\r\ngyroYaw: "), 0x80);
|
|
||||||
Notify(gyroYaw, 0x80);
|
|
||||||
Notify(PSTR("\tgyroRoll: "), 0x80);
|
|
||||||
Notify(gyroRoll, 0x80);
|
|
||||||
Notify(PSTR("\tgyroPitch: "), 0x80);
|
|
||||||
Notify(gyroPitch, 0x80);
|
|
||||||
*/
|
|
||||||
/*
|
|
||||||
Notify(PSTR("\twiimoteRoll: "), 0x80);
|
|
||||||
Notify(wiimoteRoll, 0x80);
|
|
||||||
Notify(PSTR("\twiimotePitch: "), 0x80);
|
|
||||||
Notify(wiimotePitch, 0x80);
|
|
||||||
*/
|
|
||||||
} else {
|
|
||||||
if ((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nThe gyro values has been reset"), 0x80);
|
|
||||||
#endif
|
|
||||||
gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6));
|
|
||||||
gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6));
|
|
||||||
gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6));
|
|
||||||
|
|
||||||
rollGyroScale = 500; // You might need to adjust these
|
|
||||||
pitchGyroScale = 400;
|
|
||||||
yawGyroScale = 415;
|
|
||||||
|
|
||||||
gyroYaw = 0;
|
|
||||||
gyroRoll = 0;
|
|
||||||
gyroPitch = 0;
|
|
||||||
|
|
||||||
motionValuesReset = true;
|
|
||||||
timer = micros();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
if (nunchuckConnected) {
|
|
||||||
hatValues[HatX] = l2capinbuf[15];
|
|
||||||
hatValues[HatY] = l2capinbuf[16];
|
|
||||||
accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416;
|
|
||||||
accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416;
|
|
||||||
accZ = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5)) - 416;
|
|
||||||
nunchuckPitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
|
|
||||||
nunchuckRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
|
|
||||||
}
|
|
||||||
//else if(classicControllerConnected) { }
|
|
||||||
}
|
|
||||||
if (l2capinbuf[19] & 0x01) {
|
|
||||||
if (!extensionConnected) {
|
|
||||||
extensionConnected = true;
|
|
||||||
unknownExtensionConnected = true;
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
if (extensionConnected && !unknownExtensionConnected) {
|
|
||||||
extensionConnected = false;
|
|
||||||
unknownExtensionConnected = true;
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80);
|
|
||||||
#endif
|
|
||||||
nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} else if (nunchuckConnected) {
|
|
||||||
hatValues[HatX] = l2capinbuf[15];
|
|
||||||
hatValues[HatY] = l2capinbuf[16];
|
|
||||||
accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416;
|
|
||||||
accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416;
|
|
||||||
accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416;
|
|
||||||
nunchuckPitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
|
|
||||||
nunchuckRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
|
|
||||||
|
|
||||||
pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected
|
|
||||||
roll = wiimoteRoll;
|
|
||||||
} else if (wiiUProControllerConnected) {
|
|
||||||
hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8);
|
|
||||||
hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8);
|
|
||||||
hatValues[LeftHatY] = (l2capinbuf[19] | l2capinbuf[20] << 8);
|
|
||||||
hatValues[RightHatY] = (l2capinbuf[21] | l2capinbuf[22] << 8);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
#ifdef DEBUG_USB_HOST
|
|
||||||
default:
|
|
||||||
Notify(PSTR("\r\nUnknown Report type: "), 0x80);
|
|
||||||
D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
|
|
||||||
break;
|
|
||||||
#endif
|
#endif
|
||||||
|
if (ButtonState != OldButtonState) {
|
||||||
|
ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
|
||||||
|
OldButtonState = ButtonState;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
if (l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer
|
||||||
|
accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5)) - 500;
|
||||||
|
accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500;
|
||||||
|
accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500;
|
||||||
|
wiimotePitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
|
||||||
|
wiimoteRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
|
||||||
|
}
|
||||||
|
switch (l2capinbuf[9]) {
|
||||||
|
case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV
|
||||||
|
#ifdef EXTRADEBUG
|
||||||
|
Notify(PSTR("\r\nStatus report was received"), 0x80);
|
||||||
|
#endif
|
||||||
|
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)
|
||||||
|
batteryLevel = l2capinbuf[15]; // Update battery level
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
if (l2capinbuf[12] & 0x01)
|
||||||
|
Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80);
|
||||||
|
#endif
|
||||||
|
if (checkExtension) { // If this is false it means that the user must have called getBatteryLevel()
|
||||||
|
if (l2capinbuf[12] & 0x02) { // Check if a extension is connected
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
if (!unknownExtensionConnected)
|
||||||
|
Notify(PSTR("\r\nExtension connected"), 0x80);
|
||||||
|
#endif
|
||||||
|
unknownExtensionConnected = true;
|
||||||
|
#ifdef WIICAMERA
|
||||||
|
if (!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
|
||||||
|
#endif
|
||||||
|
setReportMode(false, 0x35); // Also read the extension
|
||||||
|
} else {
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nExtension disconnected"), 0x80);
|
||||||
|
#endif
|
||||||
|
if (motionPlusConnected) {
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR(" - from Motion Plus"), 0x80);
|
||||||
|
#endif
|
||||||
|
l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
|
||||||
|
if (!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false
|
||||||
|
nunchuckConnected = false;
|
||||||
|
//else if(classicControllerConnected)
|
||||||
|
} else if (nunchuckConnected) {
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR(" - Nunchuck"), 0x80);
|
||||||
|
#endif
|
||||||
|
nunchuckConnected = false; // It must be the Nunchuck controller then
|
||||||
|
l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
|
||||||
|
onInit();
|
||||||
|
setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
|
||||||
|
} else
|
||||||
|
setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
|
||||||
|
}
|
||||||
|
} else
|
||||||
|
checkExtension = true; // Check for extensions by default
|
||||||
|
break;
|
||||||
|
case 0x21: // Read Memory Data
|
||||||
|
if ((l2capinbuf[12] & 0x0F) == 0) { // No error
|
||||||
|
// See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
|
||||||
|
if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nNunchuck connected"), 0x80);
|
||||||
|
#endif
|
||||||
|
l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED;
|
||||||
|
} else if (l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) {
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nMotion Plus connected"), 0x80);
|
||||||
|
#endif
|
||||||
|
l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED;
|
||||||
|
} else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) {
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80);
|
||||||
|
#endif
|
||||||
|
motionPlusConnected = true;
|
||||||
|
setReportMode(false, 0x35); // Also read the extension
|
||||||
|
} else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80);
|
||||||
|
#endif
|
||||||
|
activateNunchuck = false;
|
||||||
|
motionPlusConnected = true;
|
||||||
|
nunchuckConnected = true;
|
||||||
|
setReportMode(false, 0x35); // Also read the extension
|
||||||
|
} 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) {
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80);
|
||||||
|
Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80);
|
||||||
|
#endif
|
||||||
|
stateCounter = 300; // Skip the rest in "L2CAP_CHECK_MOTION_PLUS_STATE"
|
||||||
|
} else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) {
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80);
|
||||||
|
#endif
|
||||||
|
wiiUProControllerConnected = true;
|
||||||
|
}
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
else {
|
||||||
|
Notify(PSTR("\r\nUnknown Device: "), 0x80);
|
||||||
|
D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
|
||||||
|
D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
|
||||||
|
Notify(PSTR("\r\nData: "), 0x80);
|
||||||
|
for (uint8_t i = 0; i < ((l2capinbuf[12] >> 4) + 1); i++) { // bit 4-7 is the length-1
|
||||||
|
D_PrintHex<uint8_t > (l2capinbuf[15 + i], 0x80);
|
||||||
|
Notify(PSTR(" "), 0x80);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
#ifdef EXTRADEBUG
|
||||||
|
else {
|
||||||
|
Notify(PSTR("\r\nReport Error: "), 0x80);
|
||||||
|
D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
|
||||||
|
D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
case 0x22: // Acknowledge output report, return function result
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
if (l2capinbuf[13] != 0x00) { // Check if there is an error
|
||||||
|
Notify(PSTR("\r\nCommand failed: "), 0x80);
|
||||||
|
D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
case 0x30: // Core buttons - (a1) 30 BB BB
|
||||||
|
break;
|
||||||
|
case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA
|
||||||
|
pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected
|
||||||
|
roll = wiimoteRoll;
|
||||||
|
break;
|
||||||
|
case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
|
||||||
|
break;
|
||||||
|
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
|
||||||
|
pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus data available
|
||||||
|
roll = wiimoteRoll;
|
||||||
|
#ifdef WIICAMERA
|
||||||
|
// Read the IR data
|
||||||
|
IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position
|
||||||
|
IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position
|
||||||
|
IR_object_s1 = (l2capinbuf[17] & 0x0F); // size value, 0-15
|
||||||
|
|
||||||
|
IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4));
|
||||||
|
IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2));
|
||||||
|
IR_object_s2 = (l2capinbuf[20] & 0x0F);
|
||||||
|
|
||||||
|
IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4));
|
||||||
|
IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2));
|
||||||
|
IR_object_s3 = (l2capinbuf[23] & 0x0F);
|
||||||
|
|
||||||
|
IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4));
|
||||||
|
IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2));
|
||||||
|
IR_object_s4 = (l2capinbuf[26] & 0x0F);
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
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
|
||||||
|
break;
|
||||||
|
/* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */
|
||||||
|
case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes
|
||||||
|
// (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
|
||||||
|
// corresponds to output report mode 0x3e
|
||||||
|
|
||||||
|
/**** for reading in full mode: DOES NOT WORK YET ****/
|
||||||
|
/* When it works it will also have intensity and bounding box data */
|
||||||
|
/*
|
||||||
|
IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
|
||||||
|
IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
|
||||||
|
IR_object_s1 = (l2capinbuf[15] & 0x0F);
|
||||||
|
*/
|
||||||
|
break;
|
||||||
|
case 0x3F:
|
||||||
|
/*
|
||||||
|
IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
|
||||||
|
IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
|
||||||
|
IR_object_s1 = (l2capinbuf[15] & 0x0F);
|
||||||
|
*/
|
||||||
|
break;
|
||||||
|
case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes
|
||||||
|
// (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
|
||||||
|
if (motionPlusConnected) {
|
||||||
|
if (l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
|
||||||
|
if (motionValuesReset) { // We will only use the values when the gyro value has been set
|
||||||
|
gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)) - gyroYawZero);
|
||||||
|
gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero);
|
||||||
|
gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero);
|
||||||
|
|
||||||
|
yawGyroSpeed = (double)gyroYawRaw / ((double)gyroYawZero / yawGyroScale);
|
||||||
|
rollGyroSpeed = -(double)gyroRollRaw / ((double)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values
|
||||||
|
pitchGyroSpeed = (double)gyroPitchRaw / ((double)gyroPitchZero / pitchGyroScale);
|
||||||
|
|
||||||
|
/* The onboard gyro has two ranges for slow and fast mode */
|
||||||
|
if (!(l2capinbuf[18] & 0x02)) // Check if fast more is used
|
||||||
|
yawGyroSpeed *= 4.545;
|
||||||
|
if (!(l2capinbuf[18] & 0x01)) // Check if fast more is used
|
||||||
|
pitchGyroSpeed *= 4.545;
|
||||||
|
if (!(l2capinbuf[19] & 0x02)) // Check if fast more is used
|
||||||
|
rollGyroSpeed *= 4.545;
|
||||||
|
|
||||||
|
pitch = (0.93 * (pitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimotePitch); // Use a complimentary filter to calculate the angle
|
||||||
|
roll = (0.93 * (roll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimoteRoll);
|
||||||
|
|
||||||
|
gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000));
|
||||||
|
gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000));
|
||||||
|
gyroPitch += (pitchGyroSpeed * ((double)(micros() - timer) / 1000000));
|
||||||
|
timer = micros();
|
||||||
|
/*
|
||||||
|
// Uncomment these lines to tune the gyro scale variabels
|
||||||
|
Notify(PSTR("\r\ngyroYaw: "), 0x80);
|
||||||
|
Notify(gyroYaw, 0x80);
|
||||||
|
Notify(PSTR("\tgyroRoll: "), 0x80);
|
||||||
|
Notify(gyroRoll, 0x80);
|
||||||
|
Notify(PSTR("\tgyroPitch: "), 0x80);
|
||||||
|
Notify(gyroPitch, 0x80);
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
Notify(PSTR("\twiimoteRoll: "), 0x80);
|
||||||
|
Notify(wiimoteRoll, 0x80);
|
||||||
|
Notify(PSTR("\twiimotePitch: "), 0x80);
|
||||||
|
Notify(wiimotePitch, 0x80);
|
||||||
|
*/
|
||||||
|
} else {
|
||||||
|
if ((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nThe gyro values has been reset"), 0x80);
|
||||||
|
#endif
|
||||||
|
gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6));
|
||||||
|
gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6));
|
||||||
|
gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6));
|
||||||
|
|
||||||
|
rollGyroScale = 500; // You might need to adjust these
|
||||||
|
pitchGyroScale = 400;
|
||||||
|
yawGyroScale = 415;
|
||||||
|
|
||||||
|
gyroYaw = 0;
|
||||||
|
gyroRoll = 0;
|
||||||
|
gyroPitch = 0;
|
||||||
|
|
||||||
|
motionValuesReset = true;
|
||||||
|
timer = micros();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
if (nunchuckConnected) {
|
||||||
|
hatValues[HatX] = l2capinbuf[15];
|
||||||
|
hatValues[HatY] = l2capinbuf[16];
|
||||||
|
accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416;
|
||||||
|
accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416;
|
||||||
|
accZ = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5)) - 416;
|
||||||
|
nunchuckPitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
|
||||||
|
nunchuckRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
|
||||||
|
}
|
||||||
|
//else if(classicControllerConnected) { }
|
||||||
|
}
|
||||||
|
if (l2capinbuf[19] & 0x01) {
|
||||||
|
if (!extensionConnected) {
|
||||||
|
extensionConnected = true;
|
||||||
|
unknownExtensionConnected = true;
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
if (extensionConnected && !unknownExtensionConnected) {
|
||||||
|
extensionConnected = false;
|
||||||
|
unknownExtensionConnected = true;
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80);
|
||||||
|
#endif
|
||||||
|
nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (nunchuckConnected) {
|
||||||
|
hatValues[HatX] = l2capinbuf[15];
|
||||||
|
hatValues[HatY] = l2capinbuf[16];
|
||||||
|
accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416;
|
||||||
|
accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416;
|
||||||
|
accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416;
|
||||||
|
nunchuckPitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
|
||||||
|
nunchuckRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
|
||||||
|
|
||||||
|
pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected
|
||||||
|
roll = wiimoteRoll;
|
||||||
|
} else if (wiiUProControllerConnected) {
|
||||||
|
hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8);
|
||||||
|
hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8);
|
||||||
|
hatValues[LeftHatY] = (l2capinbuf[19] | l2capinbuf[20] << 8);
|
||||||
|
hatValues[RightHatY] = (l2capinbuf[21] | l2capinbuf[22] << 8);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
#ifdef DEBUG_USB_HOST
|
||||||
|
default:
|
||||||
|
Notify(PSTR("\r\nUnknown Report type: "), 0x80);
|
||||||
|
D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
|
||||||
|
break;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
L2CAP_task();
|
L2CAP_task();
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue