Fixed disconnect problem when using the old Wiimote with the external MotionPlus extension

Now also updates the batteryLevel when getBatteryLevel is called
This commit is contained in:
Kristian Sloth Lauszus 2013-07-18 19:43:21 +02:00
parent 6039f1af4b
commit d0ec18ab44
4 changed files with 72 additions and 60 deletions

51
Wii.cpp
View file

@ -14,9 +14,7 @@
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
IR camera support added by:
Allan Glover
adglover9.81@gmail.com
IR camera support added by Allan Glover (adglover9.81@gmail.com) and Kristian Lauszus
*/
#include "Wii.h"
@ -109,7 +107,7 @@ void WII::Reset() {
activateNunchuck = false;
motionValuesReset = false;
activeConnection = false;
pBtd->motionPlusInside = false;
motionPlusInside = false;
pBtd->wiiUProController = false;
wiiUProControllerConnected = false;
l2cap_event_flag = 0; // Reset flags
@ -117,13 +115,17 @@ void WII::Reset() {
}
void WII::disconnect() { // Use this void to disconnect any of the controllers
if (motionPlusConnected && !pBtd->motionPlusInside) { // Disable the Motion Plus extension
if (!motionPlusInside) { // The old Wiimote needs a delay after the first command or it will automatically reconnect
if (motionPlusConnected) {
#ifdef DEBUG_USB_HOST
Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80);
#endif
initExtension1(); // This will disable the Motion Plus extension
}
//First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection
timer = millis() + 1000; // We have to wait for the message before the rest of the channels can be deactivated
} else
timer = millis(); // Don't wait
// First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection
pBtd->l2cap_disconnection_request(hci_handle, 0x0A, interrupt_scid, interrupt_dcid);
Reset();
l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
@ -133,6 +135,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
if (!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) {
if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
motionPlusInside = pBtd->motionPlusInside;
pBtd->incomingWii = false;
pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
activeConnection = true;
@ -254,7 +257,6 @@ void WII::ACLData(uint8_t* l2capinbuf) {
#endif
} else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
//Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
if (wiimoteConnected) {
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] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
@ -291,13 +293,16 @@ void WII::ACLData(uint8_t* l2capinbuf) {
}
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
if (l2capinbuf[12] & 0x01) {
#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)
@ -327,11 +332,12 @@ void WII::ACLData(uint8_t* l2capinbuf) {
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 button and accelerometer
} else {
setReportMode(false, 0x31); // If there is no extension connected we will read the button and accelerometer
}
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
@ -351,6 +357,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
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);
@ -358,6 +365,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
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);
@ -573,7 +581,6 @@ void WII::ACLData(uint8_t* l2capinbuf) {
}
}
}
}
L2CAP_task();
}
}
@ -647,7 +654,6 @@ void WII::L2CAP_task() {
#endif
pBtd->connectToWii = false;
pBtd->pairWithWii = false;
wiimoteConnected = true;
stateCounter = 0;
l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE;
}
@ -656,7 +662,7 @@ void WII::L2CAP_task() {
/* The next states are in run() */
case L2CAP_INTERRUPT_DISCONNECT:
if (l2cap_disconnect_response_interrupt_flag) {
if (l2cap_disconnect_response_interrupt_flag && millis() > timer) {
#ifdef DEBUG_USB_HOST
Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
#endif
@ -681,11 +687,15 @@ void WII::L2CAP_task() {
}
void WII::Run() {
if (l2cap_state == L2CAP_INTERRUPT_DISCONNECT && millis() > timer)
L2CAP_task(); // Call the rest of the disconnection routine after we have waited long enough
switch (l2cap_state) {
case L2CAP_WAIT:
if (pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) {
pBtd->l2capConnectionClaimed = true;
activeConnection = true;
motionPlusInside = pBtd->motionPlusInside;
#ifdef DEBUG_USB_HOST
Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80);
#endif
@ -779,6 +789,7 @@ void WII::Run() {
case L2CAP_LED_STATE:
if (nunchuck_connected_flag)
nunchuckConnected = true;
wiimoteConnected = true;
onInit();
l2cap_state = L2CAP_DONE;
break;
@ -839,7 +850,7 @@ void WII::Run() {
/************************************************************/
void WII::HID_Command(uint8_t* data, uint8_t nbytes) {
if (pBtd->motionPlusInside)
if (motionPlusInside)
pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // It's the new wiimote with the Motion Plus Inside
else
pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]);
@ -905,6 +916,12 @@ void WII::setLedStatus() {
HID_Command(HIDBuffer, 3);
}
uint8_t WII::getBatteryLevel() {
checkExtension = false; // This is needed so the library knows that the status response is a response to this function
statusRequest(); // This will update the battery level
return batteryLevel;
};
void WII::setReportMode(bool continuous, uint8_t mode) {
uint8_t cmd_buf[4];
cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)

18
Wii.h
View file

@ -14,9 +14,7 @@
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
IR camera support added by:
Allan Glover
adglover9.81@gmail.com
IR camera support added by Allan Glover (adglover9.81@gmail.com) and Kristian Lauszus
*/
#ifndef _wii_h_
@ -215,17 +213,11 @@ public:
*/
void setLedStatus();
/**
* Call this to update battery level and Wiimote state
*/
void statusRequest();
/**
* Return the battery level of the Wiimote.
* @return The battery level in the range 0-255.
*/
uint8_t getBatteryLevel() {
return batteryLevel;
};
uint8_t getBatteryLevel();
/**
* Return the Wiimote state.
* @return See: http://wiibrew.org/wiki/Wiimote#0x20:_Status.
@ -456,6 +448,8 @@ private:
uint16_t stateCounter;
bool unknownExtensionConnected;
bool extensionConnected;
bool checkExtension; // Set to false when getBatteryLevel() is called otherwise if should be true
bool motionPlusInside; // True if it's a new Wiimote with the Motion Plus extension build into it
/* L2CAP Channels */
uint8_t control_scid[2]; // L2CAP source CID for HID_Control
@ -472,6 +466,8 @@ private:
void initExtension1();
void initExtension2();
void statusRequest(); // Used to update the Wiimote state and battery level
void readData(uint32_t offset, uint16_t size, bool EEPROM);
void readExtensionType();
void readCalData();
@ -491,7 +487,7 @@ private:
uint8_t batteryLevel;
#ifdef WIICAMERA
/* Private function and variables for the readings from teh IR Camera */
/* Private function and variables for the readings from the IR Camera */
void enableIRCamera1(); // Sets bit 2 of output report 13
void enableIRCamera2(); // Sets bit 2 of output report 1A
void writeSensitivityBlock1();

View file

@ -2,13 +2,13 @@
Example sketch for the Wii libary showing the IR camera functionality. This example
is for the Bluetooth Wii library developed for the USB shield from Circuits@Home
Created by Allan Glover and includes much from what Kristian Lauszus wrote in the existing
Wii example. Contact Kristian: http://blog.tkjelectronics.dk/ or send email at kristianl@tkjelectronics.com.
Created by Allan Glover and Kristian Lauszus.
Contact Kristian: http://blog.tkjelectronics.dk/ or send an email at kristianl@tkjelectronics.com.
Contact Allan at adglover9.81@gmail.com
To test the Wiimote IR camera, you will need access to an IR source. Sunlight will work but is not ideal.
The simpleist solution is to use the Wii sensor bar, i.e. emitter bar, supplied by the Wii system. Otherwise,
wire up a IR LED yourself.
The simpleist solution is to use the Wii sensor bar, i.e. emitter bar, supplied by the Wii system.
Otherwise, wire up a IR LED yourself.
*/
#include <Wii.h>
@ -19,7 +19,7 @@ USB Usb;
BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
/* You can create the instance of the class in two ways */
WII Wii(&Btd,PAIR); // This will start an inquiry and then pair with your Wiimote - you only have to do this once
//WII Wii(&Btd); // After the wiimote pairs once with the line of code above, you can simply create the instance like so and re upload and then press any button on the Wiimote
//WII Wii(&Btd); // After the Wiimote pairs once with the line of code above, you can simply create the instance like so and re upload and then press any button on the Wiimote
bool printAngle;
uint8_t printObjects;
@ -109,7 +109,7 @@ void loop() {
}
}
}
if(printAngle) { // There is no extension bytes avaliable, so the Motionplus or Nunchuck can't be read
if(printAngle) { // There is no extension bytes available, so the MotionPlus or Nunchuck can't be read
Serial.print(F("\r\nPitch: "));
Serial.print(Wii.getPitch());
Serial.print(F("\tRoll: "));

View file

@ -35,7 +35,6 @@ void loop() {
Serial.print(F("\r\nHOME"));
Wii[i]->disconnect();
oldControllerState[i] = false; // Reset value
delay(1000); // This delay is needed for some Wiimotes, so it doesn't try to reconnect right away
}
else {
if(Wii[i]->getButtonClick(LEFT)) {