aboutsummaryrefslogtreecommitdiff
path: root/aka.wiiremote/wiiremote.c
diff options
context:
space:
mode:
Diffstat (limited to 'aka.wiiremote/wiiremote.c')
-rw-r--r--aka.wiiremote/wiiremote.c1100
1 files changed, 1100 insertions, 0 deletions
diff --git a/aka.wiiremote/wiiremote.c b/aka.wiiremote/wiiremote.c
new file mode 100644
index 0000000..92efeb0
--- /dev/null
+++ b/aka.wiiremote/wiiremote.c
@@ -0,0 +1,1100 @@
+// wiiremote.c
+// Copyright by Masayuki Akamatsu
+// Based on "DarwiinRemote" by Hiroaki Kimura
+
+#include "wiiremote.h"
+#include <unistd.h>
+
+// this type is used a lot (data array):
+typedef unsigned char darr[];
+
+#define kTrial 10
+#define kWait 10000
+// the unit of kWait is microseconds, thus 10000 means 10ms
+
+#define kWiiIRPixelsWidth 1024.0
+#define kWiiIRPixelsHeight 768.0
+
+
+Boolean requestUpdates(WiiRemoteRef wiiremote);
+void myEventListener(IOBluetoothL2CAPChannelRef channel, void *refCon, IOBluetoothL2CAPChannelEvent *event);
+
+#define DEBUG(x)
+
+//--------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------
+
+void wiiremote_init(WiiRemoteRef wiiremote)
+{
+ wiiremote->inquiry = nil;
+ wiiremote->device = nil;
+ wiiremote->ichan = nil;
+ wiiremote->cchan = nil;
+
+ wiiremote->address = nil;
+
+ wiiremote->accX = 0x10;
+ wiiremote->accY = 0x10;
+ wiiremote->accZ = 0x10;
+ wiiremote->buttonData = 0;
+
+ wiiremote->lowZ = 0;
+ wiiremote->lowX = 0;
+ wiiremote->leftPoint = -1;
+ wiiremote->tracking = false;
+
+ wiiremote->batteryLevel = 0;
+
+ wiiremote->readingRegister = false;
+ wiiremote->isMotionSensorEnabled = false;
+ wiiremote->isVibrationEnabled = false;
+ wiiremote->isIRSensorEnabled = false;
+ wiiremote->wiiIRMode = kWiiIRModeExtended;
+ wiiremote->isExpansionPortEnabled = false;
+ wiiremote->isExpansionPortAttached = false;
+ wiiremote->expType = WiiExpNotAttached;
+
+ wiiremote->isLED1Illuminated = false;
+ wiiremote->isLED2Illuminated = false;
+ wiiremote->isLED3Illuminated = false;
+ wiiremote->isLED4Illuminated = false;
+
+ wiiremote->nAccX = 0x10;
+ wiiremote->nAccY = 0x10;
+ wiiremote->nAccZ = 0x10;
+ wiiremote->nButtonData = 0;
+
+ wiiremote->nLowZ = 0;
+ wiiremote->nLowX = 0;
+
+}
+
+//--------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------
+
+Boolean openCChan(WiiRemoteRef wiiremote)
+{
+ DEBUG(post("openCChan"););
+ short i;
+ IOReturn ret;
+
+ // open L2CAPChannel : BluetoothL2CAPPSM = 17
+ for (i=0; i<kTrial; i++)
+ {
+ ret = IOBluetoothDeviceOpenL2CAPChannelSync(wiiremote->device, &(wiiremote->cchan), 17, myEventListener, (void *)wiiremote);
+ if ( ret == kIOReturnSuccess)
+ break;
+ usleep(kWait); // wait 10ms
+ }
+ if (i==kTrial)
+ {
+ wiiremote->cchan = nil;
+ IOBluetoothDeviceCloseConnection(wiiremote->device);
+ return false;
+ }
+ IOBluetoothObjectRetain(wiiremote->cchan);
+
+ return (ret==kIOReturnSuccess);
+}
+
+Boolean openIChan(WiiRemoteRef wiiremote)
+{
+ DEBUG(post("openCChan"););
+ short i;
+ IOReturn ret;
+
+ // open L2CAPChannel : BluetoothL2CAPPSM = 19
+ for (i=0; i<kTrial; i++)
+ {
+ ret = IOBluetoothDeviceOpenL2CAPChannelSync(wiiremote->device, &(wiiremote->ichan), 19, myEventListener, (void *)wiiremote);
+ if ( ret == kIOReturnSuccess)
+ break;
+ usleep(kWait); // wait 10ms
+ }
+ if (i==kTrial)
+ {
+ wiiremote->ichan = nil;
+ IOBluetoothL2CAPChannelCloseChannel(wiiremote->cchan);
+ IOBluetoothObjectRelease(wiiremote->cchan);
+ IOBluetoothDeviceCloseConnection(wiiremote->device);
+ return false;
+ }
+ IOBluetoothObjectRetain(wiiremote->ichan);
+
+ return (ret==kIOReturnSuccess);
+}
+
+//--------------------------------------------------------------------------------------------
+
+Boolean sendCommand(WiiRemoteRef wiiremote, unsigned char *data, unsigned short length)
+{
+ unsigned char buf[40];
+ IOReturn ret;
+ int i;
+
+ memset(buf,0,40);
+ buf[0] = 0x52;
+ memcpy(buf+1, data, length);
+ if (buf[1] == 0x16)
+ length=23;
+ else
+ length++;
+
+ usleep(kWait); // wait 10ms // Done to make sure commands don't happen too fast.
+
+ for (i = 0; i<kTrial; i++)
+ {
+ ret = IOBluetoothL2CAPChannelWriteSync(wiiremote->cchan, buf, length);
+ if (ret == kIOReturnSuccess)
+ break;
+ usleep(kWait);
+ }
+
+ if (ret != kIOReturnSuccess)
+ wiiremote_disconnect(wiiremote);
+
+ return (ret==kIOReturnSuccess);
+}
+
+Boolean writeData(WiiRemoteRef wiiremote, const unsigned char *data, unsigned long address, unsigned short length)
+{
+ unsigned char cmd[22];
+ int i;
+ unsigned long addr = address;
+
+
+ for(i=0 ; i<length ; i++)
+ cmd[i+6] = data[i];
+
+ for(;i<16 ; i++)
+ cmd[i+6]= 0;
+
+ cmd[0] = 0x16;
+ cmd[1] = (addr>>24) & 0xFF;
+ cmd[2] = (addr>>16) & 0xFF;
+ cmd[3] = (addr>> 8) & 0xFF;
+ cmd[4] = (addr>> 0) & 0xFF;
+ cmd[5] = length;
+
+ // and of course the vibration flag, as usual
+ if (wiiremote->isVibrationEnabled) cmd[1] |= 0x01;
+
+ data = cmd;
+
+ return sendCommand(wiiremote, cmd, 22);
+}
+
+Boolean readData(WiiRemoteRef wiiremote, unsigned long address, unsigned short length)
+{
+
+ unsigned char cmd[7];
+ unsigned long addr = address;
+ unsigned short len = length;
+
+ cmd[0] = 0x17;
+ cmd[1] = (addr>>24)&0xFF;
+ cmd[2] = (addr>>16)&0xFF;
+ cmd[3] = (addr>> 8)&0xFF;
+ cmd[4] = (addr>> 0)&0xFF;
+
+ cmd[5] = (len >> 8)&0xFF;
+ cmd[6] = (len >> 0)&0xFF;
+
+ if (wiiremote->isVibrationEnabled) cmd[1] |= 0x01;
+ if (cmd[1] & 0x02) wiiremote->readingRegister = true;
+
+ return sendCommand(wiiremote, cmd, 7);
+}
+
+//--------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------
+
+void checkDevice(WiiRemoteRef wiiremote, IOBluetoothDeviceRef device)
+{
+ DEBUG(post("checkDevice"););
+ CFStringRef name;
+ CFStringRef address;
+
+ if (wiiremote_isconnected(wiiremote))
+ return;
+
+ name = IOBluetoothDeviceGetName(device);
+ address = IOBluetoothDeviceGetAddressString(device);
+ if (name != nil && address != nil)
+ {
+ if (CFStringCompare(name, CFSTR("Nintendo RVL-CNT-01"), 0) == kCFCompareEqualTo)
+ {
+ if ( CFStringGetLength(wiiremote->address) == 0
+ || CFStringCompare(address, wiiremote->address, kCFCompareCaseInsensitive) == kCFCompareEqualTo)
+ {
+ wiiremote->device = IOBluetoothObjectRetain(device);
+ if ( wiiremote_connect(wiiremote) == false )
+ wiiremote_disconnect(wiiremote);
+ }
+ }
+ }
+}
+
+void myFoundFunc(void *refCon, IOBluetoothDeviceInquiryRef inquiry, IOBluetoothDeviceRef device)
+{
+ DEBUG(post("myFoundFunc"););
+ checkDevice((WiiRemoteRef)refCon, device);
+}
+
+void myUpdatedFunc(void *refCon, IOBluetoothDeviceInquiryRef inquiry, IOBluetoothDeviceRef device, uint32_t devicesRemaining)
+{
+ DEBUG(post("myUpdatedFunc"););
+
+ checkDevice((WiiRemoteRef)refCon, device);
+}
+
+void myCompleteFunc(void *refCon, IOBluetoothDeviceInquiryRef inquiry, IOReturn error, Boolean aborted)
+{
+ IOReturn ret;
+ DEBUG(post("myCompleteFunc"););
+
+ if (aborted) return; // called by stop ;)
+
+ if (error != kIOReturnSuccess)
+ {
+ wiiremote_stopsearch((WiiRemoteRef)refCon);
+ return;
+ }
+/*
+ ret = IOBluetoothDeviceInquiryStart(((WiiRemoteRef)refCon)->inquiry);
+ if (ret != kIOReturnSuccess)
+ {
+ wiiremote_stopsearch((WiiRemoteRef)refCon);
+ }
+*/
+#ifdef PD
+ // PD doesn't use a CFRunLoop, so we have to manually control it
+ CFRunLoopStop( CFRunLoopGetCurrent() );
+#endif
+}
+
+//--------------------------------------------------------------------------------------------
+
+Boolean wiiremote_isconnected(WiiRemoteRef wiiremote)
+{
+ Boolean result;
+
+ result = wiiremote->device != nil && IOBluetoothDeviceIsConnected(wiiremote->device);
+ return result;
+}
+
+Boolean wiiremote_search(WiiRemoteRef wiiremote, char *address)
+{
+ DEBUG(post("wiiremote_search"););
+ IOReturn ret;
+
+ if (wiiremote->inquiry != nil)
+ return true;
+
+ wiiremote->inquiry = IOBluetoothDeviceInquiryCreateWithCallbackRefCon((void *)wiiremote);
+ IOBluetoothDeviceInquirySetDeviceFoundCallback(wiiremote->inquiry, myFoundFunc);
+ IOBluetoothDeviceInquirySetDeviceNameUpdatedCallback(wiiremote->inquiry, myUpdatedFunc);
+ IOBluetoothDeviceInquirySetCompleteCallback(wiiremote->inquiry, myCompleteFunc);
+
+ if (wiiremote->address != nil)
+ CFRelease(wiiremote->address);
+ wiiremote->address = CFStringCreateWithCString(nil, address, kCFStringEncodingMacRoman);
+
+ ret = IOBluetoothDeviceInquiryStart(wiiremote->inquiry);
+ if (ret != kIOReturnSuccess)
+ {
+ IOBluetoothDeviceInquiryDelete(wiiremote->inquiry);
+ wiiremote->inquiry = nil;
+ return false;
+ }
+#ifdef PD
+ // PD doesn't use a CFRunLoop, so we have to manually control it
+ CFRunLoopRun();
+#endif
+ return true;
+}
+
+Boolean wiiremote_stopsearch(WiiRemoteRef wiiremote)
+{
+ IOReturn ret;
+
+ if (wiiremote->inquiry == nil)
+ {
+ return true; // already stopped
+ }
+
+ ret = IOBluetoothDeviceInquiryStop(wiiremote->inquiry);
+
+ if (ret != kIOReturnSuccess && ret != kIOReturnNotPermitted)
+ {
+ // kIOReturnNotPermitted is if it's already stopped
+ }
+
+ IOBluetoothDeviceInquiryDelete(wiiremote->inquiry);
+ wiiremote->inquiry = nil;
+
+ return (ret==kIOReturnSuccess);
+}
+
+//--------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------
+
+unsigned char decrypt(unsigned char data)
+{
+ return (data ^ 0x17) + 0x17;
+}
+
+//--------------------------------------------------------------------------------------------
+
+/**
+* Handle report 0x21 (Read Data) from wiimote.
+ * dp[0] = Bluetooth header
+ * dp[1] = (0x21) Report/Channel ID
+ * dp[2] = Wiimote Buttons
+ * dp[3] = Wiimote Buttons
+ * dp[4] = High 4 bits = payload size; Low 4 bits = Error flag (0 = all good)
+ * dp[5] = Offset of memory read
+ * dp[6] = Offset of memory read
+ * dp[7+] = the Data.
+ **/
+
+void handleRAMData(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
+{
+ // specify attached expasion device
+ if ((dp[5] == 0x00) && (dp[6] == 0xF0))
+ {
+ if (decrypt(dp[21]) == 0x00)
+ {
+ wiiremote->expType = WiiNunchuk;
+ }
+ else
+ if (decrypt(dp[21]) == 0x01)
+ {
+ wiiremote->expType = WiiClassicController;
+ }
+ else
+ {
+ wiiremote->expType = WiiExpNotAttached;
+ }
+ // initExpPort = NO;
+ return;
+ }
+
+ // wiimote calibration data
+ if (!wiiremote->readingRegister && dp[5] == 0x00 && dp[6] == 0x20)
+ {
+ wiiremote->wiiCalibData.accX_zero = dp[7];
+ wiiremote->wiiCalibData.accY_zero = dp[8];
+ wiiremote->wiiCalibData.accZ_zero = dp[9];
+
+ //dp[10] - unknown/unused
+
+ wiiremote->wiiCalibData.accX_1g = dp[11];
+ wiiremote->wiiCalibData.accY_1g = dp[12];
+ wiiremote->wiiCalibData.accZ_1g = dp[13];
+ return;
+ }
+
+ // expansion device calibration data.
+ if (wiiremote->readingRegister && dp[5] == 0x00 && dp[6] == 0x20)
+ {
+ if (wiiremote->expType == WiiNunchuk)
+ {
+ //nunchuk calibration data
+ wiiremote->nunchukCalibData.accX_zero = decrypt(dp[7]);
+ wiiremote->nunchukCalibData.accY_zero = decrypt(dp[8]);
+ wiiremote->nunchukCalibData.accZ_zero = decrypt(dp[9]);
+
+ wiiremote->nunchukCalibData.accX_1g = decrypt(dp[11]);
+ wiiremote->nunchukCalibData.accY_1g = decrypt(dp[12]);
+ wiiremote->nunchukCalibData.accZ_1g = decrypt(dp[13]);
+
+ wiiremote->nunchukJoyStickCalibData.x_max = decrypt(dp[15]);
+ wiiremote->nunchukJoyStickCalibData.x_min = decrypt(dp[16]);
+ wiiremote->nunchukJoyStickCalibData.x_center = decrypt(dp[17]);
+
+ wiiremote->nunchukJoyStickCalibData.y_max = decrypt(dp[18]);
+ wiiremote->nunchukJoyStickCalibData.y_min = decrypt(dp[19]);
+ wiiremote->nunchukJoyStickCalibData.y_center = decrypt(dp[20]);
+
+ return;
+ }
+ else
+ if (wiiremote->expType == WiiClassicController)
+ {
+ //classic controller calibration data (probably)
+ }
+ }
+
+ // wii remote buttons
+ wiiremote->buttonData = ((short)dp[2] << 8) + dp[3];
+}
+
+void handleStatusReport(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
+{
+ wiiremote->batteryLevel = (double)dp[7];
+ wiiremote->batteryLevel /= (double)0xC0; // C0 = fully charged.
+
+ if ((dp[4] & 0x02)) //some device attached to Wiimote
+ {
+ wiiremote->isExpansionPortAttached = true;
+ // initExpPort = YES;
+
+ Boolean ret = writeData(wiiremote, (darr){0x00}, 0x04A40040, 1); // Initialize the device
+
+ if (ret == false)
+ {
+ wiiremote->isExpansionPortAttached = false;
+ return;
+ }
+
+ usleep(kWait); // Give the write a chance to be processed.
+
+ ret = readData(wiiremote, 0x04A400F0, 16); // read expansion device type
+ if (ret == false)
+ {
+ wiiremote->isExpansionPortAttached = false;
+ }
+ }
+ else
+ { // unplugged
+ wiiremote->isExpansionPortAttached = false;
+ wiiremote->expType = WiiExpNotAttached;
+ }
+
+ if (dp[4] & 0x10)
+ wiiremote->isLED1Illuminated = true;
+ else
+ wiiremote->isLED1Illuminated = false;
+
+ if (dp[4] & 0x20)
+ wiiremote->isLED2Illuminated = true;
+ else
+ wiiremote->isLED2Illuminated = false;
+
+ if (dp[4] & 0x40)
+ wiiremote->isLED3Illuminated = true;
+ else
+ wiiremote->isLED3Illuminated = false;
+
+ if (dp[4] & 0x80)
+ wiiremote->isLED4Illuminated = true;
+ else
+ wiiremote->isLED4Illuminated = false;
+}
+
+void handleExtensionData(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
+{
+ unsigned char startByte;
+
+ switch (dp[1]) {
+ case 0x34 :
+ startByte = 4;
+ break;
+ case 0x35 :
+ startByte = 7;
+ break;
+ case 0x36 :
+ startByte = 14;
+ break;
+ case 0x37 :
+ startByte = 17;
+ break;
+ default:
+ return; // This shouldn't ever happen.
+ break;
+ }
+
+ if (wiiremote->expType == WiiNunchuk)
+ {
+ wiiremote->nStickX = decrypt(dp[startByte]);
+ wiiremote->nStickY = decrypt(dp[startByte +1]);
+ wiiremote->nAccX = decrypt(dp[startByte +2]);
+ wiiremote->nAccY = decrypt(dp[startByte +3]);
+ wiiremote->nAccZ = decrypt(dp[startByte +4]);
+ wiiremote->nButtonData = decrypt(dp[startByte +5]);
+
+ wiiremote->nLowZ = wiiremote->nLowZ * .9 + wiiremote->nAccZ * .1;
+ wiiremote->nLowX = wiiremote->nLowX * .9 + wiiremote->nAccX * .1;
+
+ float absx = abs(wiiremote->nLowX - 128);
+ float absz = abs(wiiremote->nLowZ - 128);
+
+ if (wiiremote->nOrientation == 0 || wiiremote->nOrientation == 2) absx -= 5;
+ if (wiiremote->nOrientation == 1 || wiiremote->nOrientation == 3) absz -= 5;
+
+ if (absz >= absx)
+ {
+ if (absz > 5)
+ wiiremote->nOrientation = (wiiremote->nLowZ > 128) ? 0 : 2;
+ }
+ else
+ {
+ if (absx > 5)
+ wiiremote->nOrientation = (wiiremote->nLowX > 128) ? 3 : 1;
+ }
+ }
+ else
+ if (wiiremote->expType == WiiClassicController)
+ {
+ wiiremote->cButtonData = (unsigned short)(decrypt(dp[startByte + 4]) << 8) + decrypt(dp[startByte + 5]);
+ wiiremote->cButtonData = ~wiiremote->cButtonData; // bit reverse
+
+ wiiremote->cStickX1 = decrypt(dp[startByte]) & 0x3F;
+ wiiremote->cStickY1 = decrypt(dp[startByte + 1]) & 0x3F;
+
+ wiiremote->cStickX2 = (((decrypt(dp[startByte +0]) & 0xC0) >> 3) |
+ ((decrypt(dp[startByte +1]) & 0xC0) >> 5) |
+ ((decrypt(dp[startByte +2]) & 0x80) >> 7)) & 0x1F;
+ wiiremote->cStickY2 = decrypt(dp[startByte + 2]) & 0x1F;
+
+ wiiremote->cAnalogL = (((decrypt(dp[startByte +2]) & 0x60) >> 2) |
+ ((decrypt(dp[startByte +3]) & 0xE0) >> 5)) & 0x1F;
+ wiiremote->cAnalogR = decrypt(dp[startByte + 3]) & 0x1F;
+ }
+}
+
+void handleIRData(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
+{
+ int i;
+
+ if (dp[1] == 0x33)
+ { // 12 IR bytes
+ int startByte = 0;
+ for(i=0 ; i < 4 ; i++)
+ {
+ startByte = 7 + 3 * i;
+ wiiremote->irData[i].x = (dp[startByte +0] | ((dp[startByte +2] & 0x30) << 4)) & 0x3FF;
+ wiiremote->irData[i].y = (dp[startByte +1] | ((dp[startByte +2] & 0xC0) << 2)) & 0x3FF;
+ wiiremote->irData[i].s = dp[startByte +2] & 0x0F;
+ }
+ }
+ else
+ { // 10 IR bytes
+ int shift = (dp[1] == 0x36) ? 4 : 7;
+ int startByte = 0;
+ for (i=0; i < 2; i++) {
+ startByte = shift + 5 * i;
+ wiiremote->irData[2*i].x = (dp[startByte +0] | ((dp[startByte +2] & 0x30) << 4)) & 0x3FF;
+ wiiremote->irData[2*i].y = (dp[startByte +1] | ((dp[startByte +2] & 0xC0) << 2)) & 0x3FF;
+ wiiremote->irData[2*i].s = ((wiiremote->irData[2*i].x == wiiremote->irData[2*i].y) && (wiiremote->irData[2*i].x == 0x3FF)) ? 0x0F : 0x05; // No size is given in 10 byte report.
+
+ wiiremote->irData[(2*i)+1].x = (dp[startByte +3] | ((dp[startByte +2] & 0x03) << 8)) & 0x3FF;
+ wiiremote->irData[(2*i)+1].y = (dp[startByte +4] | ((dp[startByte +2] & 0x0C) << 6)) & 0x3FF;
+ wiiremote->irData[(2*i)+1].s = ((wiiremote->irData[(2*i)+1].x == wiiremote->irData[(2*i)+1].y) && (wiiremote->irData[(2*i)+1].x == 0x3FF)) ? 0x0F : 0x05; // No size is given in 10 byte report.
+ }
+ }
+
+ int p1 = -1;
+ int p2 = -1;
+ // we should modify this loop to take the points with the lowest s (the brightest ones)
+ for (i=0 ; i<4 ; i++) {
+ if (p1 == -1) {
+ if (wiiremote->irData [i].s < 0x0F)
+ p1 = i;
+ } else {
+ if (wiiremote->irData [i].s < 0x0F) {
+ p2 = i;
+ break;
+ }
+ }
+ }
+
+ double ox, oy;
+ if ((p1 > -1) && (p2 > -1))
+ {
+ int l = wiiremote->leftPoint;
+ if (wiiremote->leftPoint == -1)
+ {
+ switch (wiiremote->orientation)
+ {
+ case 0: l = (wiiremote->irData[p1].x < wiiremote->irData[p2].x) ? 0 : 1; break;
+ case 1: l = (wiiremote->irData[p1].y > wiiremote->irData[p2].y) ? 0 : 1; break;
+ case 2: l = (wiiremote->irData[p1].x > wiiremote->irData[p2].x) ? 0 : 1; break;
+ case 3: l = (wiiremote->irData[p1].y < wiiremote->irData[p2].y) ? 0 : 1; break;
+ }
+
+ wiiremote->leftPoint = l;
+ }
+
+ int r = 1-l;
+
+ double dx = wiiremote->irData[r].x - wiiremote->irData[l].x;
+ double dy = wiiremote->irData[r].y - wiiremote->irData[l].y;
+ double d = hypot (dx, dy);
+
+ dx /= d;
+ dy /= d;
+
+ double cx = (wiiremote->irData[l].x + wiiremote->irData[r].x)/kWiiIRPixelsWidth - 1;
+ double cy = (wiiremote->irData[l].y + wiiremote->irData[r].y)/kWiiIRPixelsHeight - 1;
+
+ ox = -dy*cy-dx*cx;
+ oy = -dx*cy+dy*cx;
+
+ // cam:
+ // Compensate for distance. There must be fewer than 0.75*768 pixels between the spots for this to work.
+ // In other words, you have to be far enough away from the sensor bar for the two spots to have enough
+ // space on the image sensor to travel without one of the points going off the image.
+ // note: it is working very well ...
+ double gain = 4;
+ if (d < (0.75 * kWiiIRPixelsHeight))
+ gain = 1 / (1 - d/kWiiIRPixelsHeight);
+
+ ox *= gain;
+ oy *= gain;
+
+ wiiremote->angle = atan2(dy, dx);
+ wiiremote->tracking = true;
+ }
+ else
+ {
+ ox = oy = -100;
+ wiiremote->leftPoint = -1; // not tracking
+ wiiremote->angle = -100;
+ wiiremote->tracking = false;
+ }
+
+ wiiremote->posX = ox;
+ wiiremote->posY = oy;
+}
+
+void handleButtonReport(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
+{
+ // wiimote buttons
+ wiiremote->buttonData = ((short)dp[2] << 8) + dp[3];
+
+ // report contains extension data
+ switch (dp[1])
+ {
+ case 0x34 :
+ case 0x35 :
+ case 0x36 :
+ case 0x37 :
+ handleExtensionData(wiiremote, dp, dataLength);
+ break;
+ }
+
+ // report contains IR data
+ if (dp[1] & 0x02)
+ {
+ handleIRData(wiiremote, dp, dataLength);
+ }
+
+ // report contains motion sensor data
+ if (dp[1] & 0x01)
+ {
+ wiiremote->accX = dp[4];
+ wiiremote->accY = dp[5];
+ wiiremote->accZ = dp[6];
+
+ wiiremote->lowZ = wiiremote->lowZ * 0.9 + wiiremote->accZ * 0.1;
+ wiiremote->lowX = wiiremote->lowX * 0.9 + wiiremote->accX * 0.1;
+
+ float absx = abs(wiiremote->lowX-128);
+ float absz = abs(wiiremote->lowZ-128);
+
+ if (wiiremote->orientation == 0 || wiiremote->orientation == 2) absx -= 5;
+ if (wiiremote->orientation == 1 || wiiremote->orientation == 3) absz -= 5;
+
+ if (absz >= absx)
+ {
+ if (absz > 5)
+ wiiremote->orientation = (wiiremote->lowZ > 128)?0:2;
+ }
+ else
+ {
+ if (absx > 5)
+ wiiremote->orientation = (wiiremote->lowX > 128)?3:1;
+ }
+ }
+}
+
+//--------------------------------------------------------------------------------------------
+
+ void myDataListener(IOBluetoothL2CAPChannelRef channel, void *dataPointer, UInt16 dataLength, void *refCon)
+{
+ DEBUG(post("myDataListener"););
+ WiiRemoteRef wiiremote = (WiiRemoteRef)refCon;
+ unsigned char* dp = (unsigned char*)dataPointer;
+
+ if (!wiiremote->device)
+ return;
+
+ //controller status (expansion port and battery level data) - received when report 0x15 sent to Wiimote (getCurrentStatus:) or status of expansion port changes.
+ if (dp[1] == 0x20 && dataLength >= 8)
+ {
+ handleStatusReport(wiiremote, dp, dataLength);
+ requestUpdates(wiiremote); // Make sure we keep getting state change reports.
+ return;
+ }
+
+ if (dp[1] == 0x21)
+ {
+ handleRAMData(wiiremote, dp, dataLength);
+ return;
+ }
+
+ if (dp[1] == 0x22)
+ { // Write data response
+ //NSLog(@"Write data response: %00x %00x %00x %00x", dp[2], dp[3], dp[4], dp[5]);
+ return;
+ }
+
+ // report contains button info
+ if ((dp[1] & 0xF0) == 0x30)
+ {
+ handleButtonReport(wiiremote, dp, dataLength);
+ }
+}
+
+void myEventListener(IOBluetoothL2CAPChannelRef channel, void *refCon, IOBluetoothL2CAPChannelEvent *event)
+{
+ DEBUG(post("myEventListener"););
+ if (event->eventType == kIOBluetoothL2CAPChannelEventTypeData)
+ {
+ // In thise case:
+ // event->u.newData.dataPtr is a pointer to the block of data received.
+ // event->u.newData.dataSize is the size of the block of data.
+ myDataListener(channel, event->u.data.dataPtr, event->u.data.dataSize, refCon);
+ }
+ else
+ if (event->eventType == kIOBluetoothL2CAPChannelEventTypeClosed)
+ {
+ // In this case:
+ // event->u.terminatedChannel is the channel that was terminated. It can be converted in an IOBluetoothL2CAPChannel
+ // object with [IOBluetoothL2CAPChannel withL2CAPChannelRef:]. (see below).
+ }
+}
+
+void myDisconnectedFunc(void * refCon, IOBluetoothUserNotificationRef inRef, IOBluetoothObjectRef objectRef)
+{
+ CFStringRef itsAddress, myAddress;
+
+ itsAddress = IOBluetoothDeviceGetAddressString(objectRef);
+ if (itsAddress != nil)
+ {
+ myAddress = IOBluetoothDeviceGetAddressString(((WiiRemoteRef)refCon)->device);
+ if (myAddress != nil)
+ {
+ if (CFStringCompare(itsAddress, myAddress, 0) == kCFCompareEqualTo)
+ {
+ wiiremote_disconnect((WiiRemoteRef)refCon);
+ }
+ CFRelease(myAddress);
+ }
+ CFRelease(itsAddress);
+ }
+}
+
+//--------------------------------------------------------------------------------------------
+
+void wiiremote_getaddress(WiiRemoteRef wiiremote, char *address)
+{
+ CFStringRef cfstring;
+
+ cfstring = IOBluetoothDeviceGetAddressString(wiiremote->device);
+ CFStringGetCString(cfstring, address, 32, kCFStringEncodingMacRoman);
+ CFRelease(cfstring);
+
+}
+
+//--------------------------------------------------------------------------------------------
+
+Boolean wiiremote_connect(WiiRemoteRef wiiremote)
+{
+ IOReturn ret;
+ Boolean result;
+ short i;
+
+ if (wiiremote->device == nil)
+ return false;
+
+ // connect the device
+ for (i=0; i<kTrial; i++)
+ {
+ ret = IOBluetoothDeviceOpenConnection(wiiremote->device, nil, nil);
+ if ( ret == kIOReturnSuccess)
+ break;
+ usleep(kWait); // wait 10ms
+ }
+ if (i==kTrial)
+ return false;
+
+ wiiremote->disconnectNotification = IOBluetoothDeviceRegisterForDisconnectNotification(wiiremote->device, myDisconnectedFunc, (void *)wiiremote);
+
+ // performs an SDP query
+ for (i=0; i<kTrial; i++)
+ {
+ ret = IOBluetoothDevicePerformSDPQuery(wiiremote->device, nil, nil);
+ if ( ret == kIOReturnSuccess)
+ break;
+ usleep(kWait); // wait 10ms
+ }
+ if (i==kTrial)
+ return false;
+
+ result = openCChan(wiiremote);
+ result = openIChan(wiiremote);
+
+ if (result)
+ {
+ result = wiiremote_led(wiiremote, wiiremote->isLED1Illuminated, wiiremote->isLED2Illuminated, wiiremote->isLED3Illuminated, wiiremote->isLED4Illuminated);
+ }
+
+ if (result == false)
+ {
+ wiiremote_disconnect(wiiremote);
+ return result;
+ }
+
+ wiiremote_getstatus(wiiremote);
+ requestUpdates(wiiremote);
+
+ readData(wiiremote, 0x0020, 7); // Get Accelerometer callibration data
+
+ return true;
+}
+
+
+Boolean wiiremote_disconnect(WiiRemoteRef wiiremote)
+{
+ short i;
+
+ if (wiiremote->cchan)
+ {
+ if (IOBluetoothDeviceIsConnected(wiiremote->device))
+ {
+ for (i=0; i<kTrial; i++)
+ {
+ if (IOBluetoothL2CAPChannelCloseChannel(wiiremote->cchan) == kIOReturnSuccess)
+ break;
+ usleep(kWait); // wait 10ms
+ }
+ }
+ if (i==kTrial) return false;
+ IOBluetoothObjectRelease(wiiremote->cchan);
+ wiiremote->cchan = nil;
+ }
+
+ if (wiiremote->ichan)
+ {
+ if (IOBluetoothDeviceIsConnected(wiiremote->device))
+ {
+ for (i=0; i<kTrial; i++)
+ {
+ if (IOBluetoothL2CAPChannelCloseChannel(wiiremote->ichan) == kIOReturnSuccess)
+ break;
+ }
+ }
+ if (i==kTrial) return false;
+ IOBluetoothObjectRelease(wiiremote->ichan);
+ wiiremote->ichan = nil;
+ }
+
+ if (wiiremote->device)
+ {
+ if (IOBluetoothDeviceIsConnected(wiiremote->device))
+ {
+ for (i=0; i<kTrial; i++)
+ {
+ if (IOBluetoothDeviceCloseConnection(wiiremote->device) == kIOReturnSuccess)
+ break;
+ }
+ }
+ if (i==kTrial) return false;
+ IOBluetoothObjectRelease(wiiremote->device);
+ wiiremote->device = nil;
+ }
+
+ if (wiiremote->disconnectNotification != nil)
+ {
+ IOBluetoothUserNotificationUnregister(wiiremote->disconnectNotification);
+ wiiremote->disconnectNotification = nil;
+ }
+
+ return true;
+}
+
+//--------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------
+
+Boolean requestUpdates(WiiRemoteRef wiiremote)
+{
+ Boolean result;
+
+ // Set the report type the Wiimote should send.
+ unsigned char cmd[] = {0x12, 0x02, 0x30}; // Just buttons.
+
+ if (wiiremote->isVibrationEnabled) cmd[1] |= 0x01;
+
+ /*
+ There are numerous status report types that can be requested.
+ The IR reports must be matched with the data format set when initializing the IR camera:
+ 0x36, 0x37 - 10 IR bytes go with Basic mode
+ 0x33 - 12 IR bytes go with Extended mode
+ 0x3e/0x3f - 36 IR bytes go with Full mode
+
+ The Nunchuk and Classic controller use 6 bytes to report their state, so the reports that
+ give more extension bytes don't provide any more info.
+
+ Buttons | Accelerometer | IR | Extension
+ --------------------+-------------------+-----------+-------------
+ 0x30: Core Buttons | | |
+ 0x31: Core Buttons | Accelerometer | |
+ 0x32: Core Buttons | | | 8 bytes
+ 0x33: Core Buttons | Accelerometer | 12 bytes |
+ 0x34: Core Buttons | | | 19 bytes
+ 0x35: Core Buttons | Accelerometer | | 16 bytes
+ 0x36: Core Buttons | | 10 bytes | 9 bytes
+ 0x37: Core Buttons | Accelerometer | 10 bytes | 6 bytes
+ ?? 0x38: Core Buttons and Accelerometer with 16 IR bytes ??
+ 0x3d: | | | 21 bytes
+
+ 0x3e / 0x3f: Interleaved Core Buttons and Accelerometer with 16/36 IR bytes
+
+ */
+
+ if (wiiremote->isIRSensorEnabled)
+ {
+ if (wiiremote->isExpansionPortEnabled)
+ {
+ cmd[2] = 0x36; // Buttons, 10 IR Bytes, 9 Extension Bytes
+ wiiremote->wiiIRMode = kWiiIRModeBasic;
+ }
+ else
+ {
+ cmd[2] = 0x33; // Buttons, Accelerometer, and 12 IR Bytes.
+ wiiremote->wiiIRMode = kWiiIRModeExtended;
+ }
+
+ // Set IR Mode
+ writeData(wiiremote, (darr){ wiiremote->wiiIRMode }, 0x04B00033, 1);
+ usleep(kWait); // wait 10ms
+ }
+ else
+ {
+ if (wiiremote->isExpansionPortEnabled)
+ {
+ cmd[2] = 0x34; // Buttons, 19 Extension Bytes
+ }
+ else
+ {
+ cmd[2] = 0x30; // Buttons
+ }
+ }
+
+ if (wiiremote->isMotionSensorEnabled) cmd[2] |= 0x01; // Add Accelerometer
+
+ usleep(kWait); // wait 10ms
+ result = sendCommand(wiiremote, cmd, 3);
+
+ return(result);
+}
+
+//--------------------------------------------------------------------------------------------
+
+Boolean wiiremote_motionsensor(WiiRemoteRef wiiremote, Boolean enabled)
+{
+ wiiremote->isMotionSensorEnabled = enabled;
+ return requestUpdates(wiiremote);
+}
+
+Boolean wiiremote_vibration(WiiRemoteRef wiiremote, Boolean enabled)
+{
+
+ wiiremote->isVibrationEnabled = enabled;
+ return requestUpdates(wiiremote);
+}
+
+Boolean wiiremote_led(WiiRemoteRef wiiremote, Boolean enabled1, Boolean enabled2, Boolean enabled3, Boolean enabled4)
+{
+ unsigned char cmd[] = {0x11, 0x00};
+ if (wiiremote->isVibrationEnabled) cmd[1] |= 0x01;
+ if (enabled1) cmd[1] |= 0x10;
+ if (enabled2) cmd[1] |= 0x20;
+ if (enabled3) cmd[1] |= 0x40;
+ if (enabled4) cmd[1] |= 0x80;
+
+ wiiremote->isLED1Illuminated = enabled1;
+ wiiremote->isLED2Illuminated = enabled2;
+ wiiremote->isLED3Illuminated = enabled3;
+ wiiremote->isLED4Illuminated = enabled4;
+
+ return sendCommand(wiiremote, cmd, 2);
+}
+
+Boolean wiiremote_expansion(WiiRemoteRef wiiremote, Boolean enabled)
+{
+ wiiremote->isExpansionPortEnabled = enabled;
+ if (wiiremote->isExpansionPortAttached == false)
+ {
+ wiiremote->isExpansionPortEnabled = false;
+ }
+ else
+ {
+ readData(wiiremote, 0x04A40020, 16); //get calbdata
+ }
+
+ return requestUpdates(wiiremote);
+}
+
+Boolean wiiremote_irsensor(WiiRemoteRef wiiremote, Boolean enabled)
+{
+ Boolean ret;
+
+ wiiremote->isIRSensorEnabled = enabled;
+
+ // ir enable 1
+ unsigned char cmd[] = {0x13, 0x00};
+ if (wiiremote->isVibrationEnabled) cmd[1] |= 0x01;
+ if (wiiremote->isIRSensorEnabled) cmd[1] |= 0x04;
+ if ((ret = sendCommand(wiiremote, cmd, 2)) == false)
+ return ret;
+ usleep(kWait);
+
+ // set register 0x1a (ir enable 2)
+ unsigned char cmd2[] = {0x1a, 0x00};
+ if (enabled) cmd2[1] |= 0x04;
+ if ((ret = sendCommand(wiiremote, cmd2, 2)) == false)
+ return ret;
+ usleep(kWait);
+
+ if(enabled)
+ {
+ // based on marcan's method, found on wiili wiki:
+ // tweaked to include some aspects of cliff's setup procedure in the hopes
+ // of it actually turning on 100% of the time (was seeing 30-40% failure rate before)
+ // the sleeps help it it seems
+ usleep(kWait);
+ if ((ret = writeData(wiiremote, (darr){0x01}, 0x04B00030, 1)) == false) return ret;
+ usleep(kWait);
+ if ((ret = writeData(wiiremote, (darr){0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0xC0}, 0x04B00000, 9)) == false) return ret;
+ usleep(kWait);
+ if ((ret = writeData(wiiremote, (darr){0x40, 0x00}, 0x04B0001A, 2)) == false) return ret;
+ usleep(kWait);
+ if ((ret = writeData(wiiremote, (darr){0x08}, 0x04B00030, 1)) == false) return ret;
+ usleep(kWait);
+
+ requestUpdates(wiiremote);
+ }
+ else
+ {
+ // probably should do some writes to power down the camera, save battery
+ // but don't know how yet.
+
+ ret = wiiremote_motionsensor(wiiremote, wiiremote->isMotionSensorEnabled);
+ ret = wiiremote_vibration(wiiremote, wiiremote->isVibrationEnabled);
+ ret = wiiremote_expansion(wiiremote, wiiremote->isExpansionPortEnabled);
+ }
+
+ return ret;
+}
+
+Boolean wiiremote_getstatus(WiiRemoteRef wiiremote)
+{
+ unsigned char cmd[] = {0x15, 0x00};
+ return sendCommand(wiiremote, cmd, 2);
+}
+
+