/[soapbox]/misc.wiiremote/trunk/wiimote.cpp
ViewVC logotype

Contents of /misc.wiiremote/trunk/wiimote.cpp

Parent Directory Parent Directory | Revision Log Revision Log


Revision 3129 - (show annotations) (download)
Sat May 21 17:14:31 2011 UTC (11 years, 1 month ago) by tadas
File size: 91039 byte(s)
Something working
1 // _______________________________________________________________________________
2 //
3 // - WiiYourself! - native C++ Wiimote library v1.15
4 // (c) gl.tter 2007-10 - http://gl.tter.org
5 //
6 // see License.txt for conditions of use. see History.txt for change log.
7 // _______________________________________________________________________________
8 //
9 // wiimote.cpp (tab = 4 spaces)
10
11 // VC-specifics:
12 #ifdef _MSC_VER
13 // disable warning "C++ exception handler used, but unwind semantics are not enabled."
14 // in <xstring> (I don't use it - or just enable C++ exceptions)
15 # pragma warning(disable: 4530)
16 // auto-link with the necessary libs
17 # pragma comment(lib, "setupapi.lib")
18 # pragma comment(lib, "hid.lib") // for HID API (from DDK)
19 # pragma comment(lib, "winmm.lib") // for timeGetTime()
20 #endif // _MSC_VER
21
22 #include "wiimote.h"
23 #include <setupapi.h>
24 extern "C" {
25 # ifdef __MINGW32__
26 # include <ddk/hidsdi.h>// from WinDDK
27 # else
28 # include <api/hidsdi.h>
29 # endif
30 }
31 #include <sys/types.h> // for _stat
32 #include <sys/stat.h> // "
33 #include <process.h> // for _beginthreadex()
34 #ifdef __BORLANDC__
35 # include <cmath.h> // for orientation
36 #else
37 # include <math.h> // "
38 #endif
39 #include <mmreg.h> // for WAVEFORMATEXTENSIBLE
40 #include <mmsystem.h> // for timeGetTime()
41
42 #include "ext.h" // FIXME: nx cia
43
44 // apparently not defined in some compilers:
45 #ifndef min
46 # define min(a,b) (((a) < (b)) ? (a) : (b))
47 #endif
48 // ------------------------------------------------------------------------------------
49 // helpers
50 // ------------------------------------------------------------------------------------
51 template<class T> inline T sign (const T& val) { return (val<0)? T(-1) : T(1); }
52 template<class T> inline T square(const T& val) { return val*val; }
53 #define ARRAY_ENTRIES(array) (sizeof(array)/sizeof(array[0]))
54
55 // ------------------------------------------------------------------------------------
56 // Tracing & Debugging
57 // ------------------------------------------------------------------------------------
58 #define PREFIX _T("WiiYourself! : ")
59
60 // comment these to auto-strip their code from the library:
61 // (they currently use OutputDebugString() via _TRACE() - change to suit)
62 /*#if (_MSC_VER >= 1400) // VC 2005+ (earlier versions don't support variable args)
63 # define TRACE(fmt, ...) _TRACE(PREFIX fmt _T("\n"), __VA_ARGS__)
64 # define WARN(fmt, ...) _TRACE(PREFIX _T("* ") fmt _T(" *") _T("\n"), __VA_ARGS__)
65 #elif defined(__MINGW32__)
66 # define TRACE(fmt, ...) _TRACE(PREFIX fmt _T("\n") , ##__VA_ARGS__)
67 # define WARN(fmt, ...) _TRACE(PREFIX _T("* ") fmt _T(" *") _T("\n") , ##__VA_ARGS__)
68 #endif*/
69 # define TRACE(fmt, ...) post((PREFIX fmt _T("\n")), __VA_ARGS__)
70 # define WARN(fmt, ...) post((PREFIX _T("* ") fmt _T(" *") _T("\n")), __VA_ARGS__)
71 // uncomment any of these for deeper debugging:
72 //#define DEEP_TRACE(fmt, ...) post(PREFIX _T("|") fmt _T("\n"), __VA_ARGS__) // VC 2005+
73 //#define DEEP_TRACE(fmt, ...) post(PREFIX _T("|") fmt _T("\n") , ##__VA_ARGS__) // mingw
74 //#define BEEP_DEBUG_READS
75 //#define BEEP_DEBUG_WRITES
76 //#define BEEP_ON_ORIENTATION_ESTIMATE
77 //#define BEEP_ON_PERIODIC_STATUSREFRESH
78
79 // internals: auto-strip code from the macros if they weren't defined
80 #ifndef TRACE
81 # define TRACE
82 #endif
83 #ifndef DEEP_TRACE
84 # define DEEP_TRACE
85 #endif
86 #ifndef WARN
87 # define WARN
88 #endif
89 // ------------------------------------------------------------------------------------
90 static void _cdecl _TRACE (const TCHAR* fmt, ...)
91 {
92 static TCHAR buffer[256];
93 if (!fmt) return;
94
95 va_list argptr;
96 va_start (argptr, fmt);
97 #if (_MSC_VER >= 1400) // VC 2005+
98 _vsntprintf_s(buffer, ARRAY_ENTRIES(buffer), _TRUNCATE, fmt, argptr);
99 #else
100 _vsntprintf (buffer, ARRAY_ENTRIES(buffer), fmt, argptr);
101 #endif
102 va_end (argptr);
103
104 OutputDebugString(buffer);
105 }
106
107 // ------------------------------------------------------------------------------------
108 // wiimote
109 // ------------------------------------------------------------------------------------
110 // class statics
111 HMODULE wiimote::HidDLL = NULL;
112 unsigned wiimote::_TotalCreated = 0;
113 unsigned wiimote::_TotalConnected = 0;
114 hidwrite_ptr wiimote::_HidD_SetOutputReport = NULL;
115
116 // (keep in sync with 'speaker_freq'):
117 const unsigned wiimote::FreqLookup [TOTAL_FREQUENCIES] =
118 { 0, 4200, 3920, 3640, 3360,
119 3130, 2940, 2760, 2610, 2470 };
120
121 const TCHAR* wiimote::ButtonNameFromBit [TOTAL_BUTTON_BITS] =
122 { _T("Left") , _T("Right"), _T("Down"), _T("Up"),
123 _T("Plus") , _T("??") , _T("??") , _T("??") ,
124 _T("Two") , _T("One") , _T("B") , _T("A") ,
125 _T("Minus"), _T("??") , _T("??") , _T("Home") };
126
127 const TCHAR* wiimote::ClassicButtonNameFromBit [TOTAL_BUTTON_BITS] =
128 { _T("??") , _T("TrigR") , _T("Plus") , _T("Home"),
129 _T("Minus"), _T("TrigL") , _T("Down") , _T("Right") ,
130 _T("Up") , _T("Left") , _T("ZR") , _T("X") ,
131 _T("A") , _T("Y") , _T("B") , _T("ZL") };
132 // ------------------------------------------------------------------------------------
133 wiimote::wiimote ()
134 :
135 DataRead (CreateEvent(NULL, FALSE, FALSE, NULL)),
136 Handle (INVALID_HANDLE_VALUE),
137 ReportType (IN_BUTTONS),
138 bStatusReceived (false), // for output method detection
139 bConnectInProgress (true ),
140 bInitInProgress (false),
141 bEnablingMotionPlus (false),
142 bConnectionLost (false), // set if write fails after connection
143 bMotionPlusDetected (false),
144 bMotionPlusEnabled (false),
145 bMotionPlusExtension (false),
146 bCalibrateAtRest (false),
147 bUseHIDwrite (false), // if OS supports it
148 ChangedCallback (NULL),
149 CallbackTriggerFlags (CHANGED_ALL),
150 InternalChanged (NO_CHANGE),
151 CurrentSample (NULL),
152 HIDwriteThread (NULL),
153 ReadParseThread (NULL),
154 SampleThread (NULL),
155 AsyncRumbleThread (NULL),
156 AsyncRumbleTimeout (0),
157 UniqueID (0) // not _guaranteed_ unique, see comments in header
158 #ifdef ID2_FROM_DEVICEPATH // (see comments in header)
159 // UniqueID2 (0)
160 #endif
161 {
162 _ASSERT(DataRead != INVALID_HANDLE_VALUE);
163
164 // if this is the first wiimote object, detect & enable HID write support
165 if(++_TotalCreated == 1)
166 {
167 HidDLL = LoadLibrary(_T("hid.dll"));
168 _ASSERT(HidDLL);
169 if(!HidDLL)
170 WARN(_T("Couldn't load hid.dll - shouldn't happen!"));
171 else{
172 _HidD_SetOutputReport = (hidwrite_ptr)
173 GetProcAddress(HidDLL, "HidD_SetOutputReport");
174 if(_HidD_SetOutputReport)
175 TRACE(_T("OS supports HID writes."));
176 else
177 TRACE(_T("OS doesn't support HID writes."));
178 }
179 }
180
181 // clear our public and private state data completely (including deadzones)
182 Clear (true);
183 Internal.Clear(true);
184
185 // and the state recording vars
186 memset(&Recording, 0, sizeof(Recording));
187
188 // for overlapped IO (Read/WriteFile)
189 memset(&Overlapped, 0, sizeof(Overlapped));
190 Overlapped.hEvent = DataRead;
191 Overlapped.Offset =
192 Overlapped.OffsetHigh = 0;
193
194 // for async HID output method
195 InitializeCriticalSection(&HIDwriteQueueLock);
196 // for polling
197 InitializeCriticalSection(&StateLock);
198
199 // request millisecond timer accuracy
200 timeBeginPeriod(1);
201 }
202 // ------------------------------------------------------------------------------------
203 wiimote::~wiimote ()
204 {
205 Disconnect();
206
207 // events & critical sections are kept open for the lifetime of the object,
208 // so tidy them up here:
209 if(DataRead != INVALID_HANDLE_VALUE)
210 CloseHandle(DataRead);
211
212 DeleteCriticalSection(&HIDwriteQueueLock);
213 DeleteCriticalSection(&StateLock);
214
215 // tidy up timer accuracy request
216 timeEndPeriod(1);
217
218 // release HID DLL (for dynamic HID write method)
219 if((--_TotalCreated == 0) && HidDLL)
220 {
221 FreeLibrary(HidDLL);
222 HidDLL = NULL;
223 _HidD_SetOutputReport = NULL;
224 }
225 }
226
227 // ------------------------------------------------------------------------------------
228 bool wiimote::Connect (unsigned wiimote_index, bool force_hidwrites)
229 {
230 if(wiimote_index == FIRST_AVAILABLE)
231 TRACE(_T("Connecting first available Wiimote:"));
232 else
233 TRACE(_T("Connecting Wiimote %u:"), wiimote_index);
234
235 // auto-disconnect if user is being naughty
236 if(IsConnected())
237 Disconnect();
238
239 // get the GUID of the HID class
240 GUID guid;
241 HidD_GetHidGuid(&guid);
242
243 // get a handle to all devices that are part of the HID class
244 // Brian: Fun fact: DIGCF_PRESENT worked on my machine just fine. I reinstalled
245 // Vista, and now it no longer finds the Wiimote with that parameter enabled...
246 HDEVINFO dev_info = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_DEVICEINTERFACE);// | DIGCF_PRESENT);
247 if(!dev_info) {
248 WARN(_T("couldn't get device info"));
249 return false;
250 }
251
252 // enumerate the devices
253 SP_DEVICE_INTERFACE_DATA didata;
254 didata.cbSize = sizeof(didata);
255
256 unsigned index = 0;
257 unsigned wiimotes_found = 0;
258 while(SetupDiEnumDeviceInterfaces(dev_info, NULL, &guid, index, &didata))
259 {
260 // get the buffer size for this device detail instance
261 DWORD req_size = 0;
262 SetupDiGetDeviceInterfaceDetail(dev_info, &didata, NULL, 0, &req_size, NULL);
263
264 // (bizarre way of doing it) create a buffer large enough to hold the
265 // fixed-size detail struct components, and the variable string size
266 SP_DEVICE_INTERFACE_DETAIL_DATA *didetail =
267 (SP_DEVICE_INTERFACE_DETAIL_DATA*) new BYTE[req_size];
268 _ASSERT(didetail);
269 didetail->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA);
270
271 // now actually get the detail struct
272 if(!SetupDiGetDeviceInterfaceDetail(dev_info, &didata, didetail,
273 req_size, &req_size, NULL)) {
274 WARN(_T("couldn't get devinterface info for %u"), index);
275 break;
276 }
277
278 // open a shared handle to the device to query it (this will succeed even
279 // if the wiimote is already Connect()'ed)
280 DEEP_TRACE(_T(".. querying device %s"), didetail->DevicePath);
281 Handle = CreateFile(didetail->DevicePath, 0, FILE_SHARE_READ|FILE_SHARE_WRITE,
282 NULL, OPEN_EXISTING, 0, NULL);
283 if(Handle == INVALID_HANDLE_VALUE) {
284 DEEP_TRACE(_T(".... failed with err %x (probably harmless)."),
285 GetLastError());
286 goto skip;
287 }
288
289 // get the device attributes
290 HIDD_ATTRIBUTES attrib;
291 attrib.Size = sizeof(attrib);
292 if(HidD_GetAttributes(Handle, &attrib))
293 {
294 // is this a wiimote?
295 if((attrib.VendorID != VID) || (attrib.ProductID != PID))
296 goto skip;
297
298 // yes, but is it the one we're interested in?
299 ++wiimotes_found;
300 if((wiimote_index != FIRST_AVAILABLE) &&
301 (wiimote_index != wiimotes_found))
302 goto skip;
303
304 // the wiimote is installed, but it may not be currently paired:
305 if(wiimote_index == FIRST_AVAILABLE)
306 TRACE(_T(".. opening Wiimote %u:"), wiimotes_found);
307 else
308 TRACE(_T(".. opening:"));
309
310
311 // re-open the handle, but this time we don't allow write sharing
312 // (that way subsequent calls can still _discover_ wiimotes above, but
313 // will correctly fail here if they're already connected)
314 CloseHandle(Handle);
315
316 // note this also means that if another application has already opened
317 // the device, the library can no longer connect it (this may happen
318 // with software that enumerates all joysticks in the system, because
319 // even though the wiimote is not a standard joystick (and can't
320 // be read as such), it unfortunately announces itself to the OS
321 // as one. The SDL library was known to do grab wiimotes like this.
322 // If you cannot stop the application from doing it, you may change the
323 // call below to open the device in full shared mode - but then the
324 // library can no longer detect if you've already connected a device
325 // and will allow you to connect it twice! So be careful ...
326 Handle = CreateFile(didetail->DevicePath, GENERIC_READ|GENERIC_WRITE,
327 FILE_SHARE_READ,
328 NULL, OPEN_EXISTING,
329 FILE_FLAG_OVERLAPPED, NULL);
330 if(Handle == INVALID_HANDLE_VALUE) {
331 TRACE(_T(".... failed with err %x"), GetLastError());
332 goto skip;
333 }
334
335 // clear the wiimote state & buffers
336 Clear (false); // preserves existing deadzones
337 Internal.Clear(false); // "
338 InternalChanged = NO_CHANGE;
339 memset(ReadBuff , 0, sizeof(ReadBuff));
340 bConnectionLost = false;
341 bConnectInProgress = true; // don't parse extensions or request regular
342 // updates until complete
343 // enable async reading
344 BeginAsyncRead();
345
346 // autodetect which write method the Bluetooth stack supports,
347 // by requesting the wiimote status report:
348 if(force_hidwrites && !_HidD_SetOutputReport) {
349 TRACE(_T(".. can't force HID writes (not supported)"));
350 force_hidwrites = false;
351 }
352
353 if(force_hidwrites)
354 TRACE(_T(".. (HID writes forced)"));
355 else{
356 // - try WriteFile() first as it's the most efficient (it uses
357 // harware interrupts where possible and is async-capable):
358 bUseHIDwrite = false;
359 RequestStatusReport();
360 // and wait for the report to arrive:
361 DWORD last_time = timeGetTime();
362 while(!bStatusReceived && ((timeGetTime()-last_time) < 500))
363 Sleep(10);
364 TRACE(_T(".. WriteFile() %s."), bStatusReceived? _T("succeeded") :
365 _T("failed"));
366 }
367
368 // try HID write method (if supported)
369 if(!bStatusReceived && _HidD_SetOutputReport)
370 {
371 bUseHIDwrite = true;
372 RequestStatusReport();
373 // wait for the report to arrive:
374 DWORD last_time = timeGetTime();
375 while(!bStatusReceived && ((timeGetTime()-last_time) < 500))
376 Sleep(10);
377 // did we get it?
378 TRACE(_T(".. HID write %s."), bStatusReceived? _T("succeeded") :
379 _T("failed"));
380 }
381
382 // still failed?
383 if(!bStatusReceived) {
384 WARN(_T("output failed - wiimote is not connected (or confused)."));
385 Disconnect();
386 goto skip;
387 }
388
389 //Sleep(500);
390 // reset it
391 Reset();
392
393 // read the wiimote calibration info
394 ReadCalibration();
395
396 // allow the result(s) to come in (so that the caller can immediately test
397 // MotionPlusConnected()
398 Sleep(300); // note, don't need it on my system, better to be safe though
399
400 // connected succesfully:
401 _TotalConnected++;
402
403 // use the first incomding analogue sensor values as the 'at rest'
404 // offsets (only supports the Balance Board currently)
405 bCalibrateAtRest = true;
406
407 // refresh the public state from the internal one (so that everything
408 // is available straight away
409 RefreshState();
410
411 // attempt to construct a unique hardware ID from the calibration
412 // data bytes (this is obviously not guaranteed to be unique across
413 // all devices, but may work fairly well in practice... ?)
414 memcpy(&UniqueID, &CalibrationInfo, sizeof(CalibrationInfo));
415
416 _ASSERT(UniqueID != 0); // if this fires, the calibration data didn't
417 // arrive - this shouldn't happen
418
419 #ifdef ID2_FROM_DEVICEPATH // (see comments in header)
420 // create a 2nd alternative id by simply adding all the characters
421 // in the device path to create a single number
422 UniqueID2 = 0;
423 for(unsigned index=0; index<_tcslen(didetail->DevicePath); index++)
424 UniqueID2 += didetail->DevicePath[index];
425 #endif
426 // and show when we want to trigger the next periodic status request
427 // (for battery level and connection loss detection)
428 NextStatusTime = timeGetTime() + REQUEST_STATUS_EVERY_MS;
429 NextMPlusDetectTime = timeGetTime() + DETECT_MPLUS_EVERY_MS;
430 MPlusDetectCount = DETECT_MPLUS_COUNT;
431
432 // tidy up
433 delete[] (BYTE*)didetail;
434 break;
435 }
436 skip:
437 // tidy up
438 delete[] (BYTE*)didetail;
439
440 if(Handle != INVALID_HANDLE_VALUE) {
441 CloseHandle(Handle);
442 Handle = INVALID_HANDLE_VALUE;
443 }
444 // if this was the specified wiimote index, abort
445 if((wiimote_index != FIRST_AVAILABLE) &&
446 (wiimote_index == (wiimotes_found-1)))
447 break;
448
449 index++;
450 }
451
452 // clean up our list
453 SetupDiDestroyDeviceInfoList(dev_info);
454
455 bConnectInProgress = false;
456 if(IsConnected()) {
457 TRACE(_T(".. connected!"));
458 // notify the callbacks (if requested to do so)
459 if(CallbackTriggerFlags & CONNECTED)
460 {
461 ChangedNotifier(CONNECTED, Internal);
462 if(ChangedCallback)
463 ChangedCallback(*this, CONNECTED, Internal);
464 }
465 return true;
466 }
467 TRACE(_T(".. connection failed."));
468 return false;
469 }
470 // ------------------------------------------------------------------------------------
471 void wiimote::CalibrateAtRest ()
472 {
473 _ASSERT(IsConnected());
474 if(!IsConnected())
475 return;
476
477 // the app calls this to remove 'at rest' offsets from the analogue sensor
478 // values (currently only works for the Balance Board):
479 if(IsBalanceBoard()) {
480 TRACE(_T(".. removing 'at rest' BBoard offsets."));
481 Internal.BalanceBoard.AtRestKg = Internal.BalanceBoard.Kg;
482 RefreshState();
483 }
484 }
485 // ------------------------------------------------------------------------------------
486 void wiimote::Disconnect ()
487 {
488 if(Handle == INVALID_HANDLE_VALUE)
489 return;
490
491 TRACE(_T("Disconnect()."));
492
493 if(IsConnected())
494 {
495 _ASSERT(_TotalConnected > 0); // sanity
496 _TotalConnected--;
497
498 if(!bConnectionLost)
499 Reset();
500 }
501
502 CloseHandle(Handle);
503 Handle = INVALID_HANDLE_VALUE;
504 UniqueID = 0;
505 #ifdef ID2_FROM_DEVICEPATH // (see comments in header)
506 UniqueID2 = 0;
507 #endif
508
509 // close the read thread
510 if(ReadParseThread) {
511 // unblock it so it can realise we're closing and exit straight away
512 SetEvent(DataRead);
513 WaitForSingleObject(ReadParseThread, 3000);
514 CloseHandle(ReadParseThread);
515 ReadParseThread = NULL;
516 }
517 // close the rumble thread
518 if(AsyncRumbleThread) {
519 WaitForSingleObject(AsyncRumbleThread, 3000);
520 CloseHandle(AsyncRumbleThread);
521 AsyncRumbleThread = NULL;
522 AsyncRumbleTimeout = 0;
523 }
524 // and the sample streaming thread
525 if(SampleThread) {
526 WaitForSingleObject(SampleThread, 3000);
527 CloseHandle(SampleThread);
528 SampleThread = NULL;
529 }
530
531 #ifndef USE_DYNAMIC_HIDQUEUE
532 HID.Deallocate();
533 #endif
534
535 bStatusReceived = false;
536
537 // and clear the state
538 Clear (false); // (preserves deadzones)
539 Internal.Clear(false); // "
540 InternalChanged = NO_CHANGE;
541 }
542 // ------------------------------------------------------------------------------------
543 void wiimote::Reset ()
544 {
545 TRACE(_T("Resetting wiimote."));
546
547 if(bMotionPlusEnabled)
548 DisableMotionPlus();
549
550 // stop updates (by setting report type to non-continuous, buttons-only)
551 if(IsBalanceBoard())
552 SetReportType(IN_BUTTONS_BALANCE_BOARD, false);
553 else
554 SetReportType(IN_BUTTONS, false);
555
556 SetRumble (false);
557 SetLEDs (0x00);
558 // MuteSpeaker (true);
559 EnableSpeaker(false);
560
561 Sleep(150); // avoids loosing the extension calibration data on Connect()
562 }
563 // ------------------------------------------------------------------------------------
564 unsigned __stdcall wiimote::ReadParseThreadfunc (void* param)
565 {
566 // this thread waits for the async ReadFile() to deliver data & parses it.
567 // it also requests periodic status updates, deals with connection loss
568 // and ends state recordings with a specific duration:
569 _ASSERT(param);
570 wiimote &remote = *(wiimote*)param;
571 OVERLAPPED &overlapped = remote.Overlapped;
572 unsigned exit_code = 0; // (success)
573
574 while(1)
575 {
576 // wait until the overlapped read completes, or the timeout is reached:
577 DWORD wait = WaitForSingleObject(overlapped.hEvent, 500);
578
579 // before we deal with the result, let's do some housekeeping:
580
581 // if we were recently Disconect()ed, exit now
582 if(remote.Handle == INVALID_HANDLE_VALUE) {
583 DEEP_TRACE(_T("read thread: wiimote was disconnected"));
584 break;
585 }
586 // ditto if the connection was lost (eg. through a failed write)
587 if(remote.bConnectionLost)
588 {
589 connection_lost:
590 TRACE(_T("read thread: connection to wiimote was lost"));
591 remote.Disconnect();
592 remote.InternalChanged = (state_change_flags)
593 (remote.InternalChanged | CONNECTION_LOST);
594 // report via the callback (if any)
595 if(remote.CallbackTriggerFlags & CONNECTION_LOST)
596 {
597 remote.ChangedNotifier(CONNECTION_LOST, remote.Internal);
598 if(remote.ChangedCallback)
599 remote.ChangedCallback(remote, CONNECTION_LOST, remote.Internal);
600 }
601 break;
602 }
603
604 DWORD time = timeGetTime();
605 // periodic events (but not if we're streaming audio,
606 // we don't want to cause a glitch)
607 if(remote.IsConnected() && !remote.bInitInProgress &&
608 !remote.IsPlayingAudio())
609 {
610 // status request due?
611 if(time > remote.NextStatusTime)
612 {
613 #ifdef BEEP_ON_PERIODIC_STATUSREFRESH
614 Beep(2000,50);
615 #endif
616 remote.RequestStatusReport();
617 // and schedule the next one
618 remote.NextStatusTime = time + REQUEST_STATUS_EVERY_MS;
619 }
620 // motion plus detection due?
621 if(!remote.IsBalanceBoard() &&
622 // !remote.bConnectInProgress &&
623 !remote.bMotionPlusExtension &&
624 (remote.Internal.ExtensionType != MOTION_PLUS) &&
625 (remote.Internal.ExtensionType != PARTIALLY_INSERTED) &&
626 (time > remote.NextMPlusDetectTime))
627 {
628 remote.DetectMotionPlusExtensionAsync();
629 // we try several times in quick succession before the next
630 // delay:
631 if(--remote.MPlusDetectCount == 0) {
632 remote.NextMPlusDetectTime = time + DETECT_MPLUS_EVERY_MS;
633 remote.MPlusDetectCount = DETECT_MPLUS_COUNT;
634 #ifdef _DEBUG
635 TRACE(_T("--"));
636 #endif
637 }
638 }
639 }
640
641 // if we're state recording and have reached the specified duration, stop
642 if(remote.Recording.bEnabled && (remote.Recording.EndTimeMS != UNTIL_STOP) &&
643 (time >= remote.Recording.EndTimeMS))
644 remote.Recording.bEnabled = false;
645
646 // now handle the wait result:
647 // did the wait time out?
648 if(wait == WAIT_TIMEOUT) {
649 DEEP_TRACE(_T("read thread: timed out"));
650 continue; // wait again
651 }
652 // did an error occurr?
653 if(wait != WAIT_OBJECT_0) {
654 DEEP_TRACE(_T("read thread: error waiting!"));
655 remote.bConnectionLost = true;
656 // deal with it straight away to avoid a longer delay
657 goto connection_lost;
658 }
659
660 // data was received:
661 #ifdef BEEP_DEBUG_READS
662 Beep(500,1);
663 #endif
664 DWORD read = 0;
665 // get the data read result
666 GetOverlappedResult(remote.Handle, &overlapped, &read, TRUE);
667 // if we read data, parse it
668 if(read) {
669 DEEP_TRACE(_T("read thread: parsing data"));
670 remote.OnReadData(read);
671 }
672 else
673 DEEP_TRACE(_T("read thread: didn't get any data??"));
674 }
675
676 TRACE(_T("(ending read thread)"));
677 #ifdef BEEP_DEBUG_READS
678 if(exit_code != 0)
679 Beep(200,1000);
680 #endif
681 return exit_code;
682 }
683 // ------------------------------------------------------------------------------------
684 bool wiimote::BeginAsyncRead ()
685 {
686 // (this is also called before we're fully connected)
687 if(Handle == INVALID_HANDLE_VALUE)
688 return false;
689
690 DEEP_TRACE(_T(".. starting async read"));
691 #ifdef BEEP_DEBUG_READS
692 Beep(1000,1);
693 #endif
694
695 DWORD read;
696 if (!ReadFile(Handle, ReadBuff, REPORT_LENGTH, &read, &Overlapped)) {
697 DWORD err = GetLastError();
698 if(err != ERROR_IO_PENDING) {
699 DEEP_TRACE(_T(".... ** ReadFile() failed! **"));
700 return false;
701 }
702 }
703
704 // launch the completion wait/callback thread
705 if(!ReadParseThread) {
706 ReadParseThread = (HANDLE)_beginthreadex(NULL, 0, ReadParseThreadfunc,
707 this, 0, NULL);
708 DEEP_TRACE(_T(".... creating read thread"));
709 _ASSERT(ReadParseThread);
710 if(!ReadParseThread)
711 return false;
712 SetThreadPriority(ReadParseThread, WORKER_THREAD_PRIORITY);
713 }
714
715 // if ReadFile completed while we called, signal the thread to proceed
716 if(read) {
717 DEEP_TRACE(_T(".... got data right away"));
718 SetEvent(DataRead);
719 }
720 return true;
721 }
722 // ------------------------------------------------------------------------------------
723 void wiimote::OnReadData (DWORD bytes_read)
724 {
725 _ASSERT(bytes_read == REPORT_LENGTH);
726
727 // copy our input buffer
728 BYTE buff [REPORT_LENGTH];
729 memcpy(buff, ReadBuff, bytes_read);
730
731 // start reading again
732 BeginAsyncRead();
733
734 // parse it
735 ParseInput(buff);
736 }
737 // ------------------------------------------------------------------------------------
738 void wiimote::SetReportType (input_report type, bool continuous)
739 {
740 _ASSERT(IsConnected());
741 if(!IsConnected())
742 return;
743
744 // the balance board only uses one type of report
745 _ASSERT(!IsBalanceBoard() || type == IN_BUTTONS_BALANCE_BOARD);
746 if(IsBalanceBoard() && (type != IN_BUTTONS_BALANCE_BOARD))
747 return;
748
749 #ifdef TRACE
750 #define TYPE2NAME(_type) (type==_type)? _T(#_type)
751 const TCHAR* name = TYPE2NAME(IN_BUTTONS) :
752 TYPE2NAME(IN_BUTTONS_ACCEL_IR) :
753 TYPE2NAME(IN_BUTTONS_ACCEL_EXT) :
754 TYPE2NAME(IN_BUTTONS_ACCEL_IR_EXT) :
755 TYPE2NAME(IN_BUTTONS_BALANCE_BOARD) :
756 _T("(unknown??)");
757 TRACE(_T("ReportType: %s (%s)"), name, (continuous? _T("continuous") :
758 _T("non-continuous")));
759 #endif
760 ReportType = type;
761
762 switch(type)
763 {
764 case IN_BUTTONS_ACCEL_IR:
765 EnableIR(wiimote_state::ir::EXTENDED);
766 break;
767 case IN_BUTTONS_ACCEL_IR_EXT:
768 EnableIR(wiimote_state::ir::BASIC);
769 break;
770 default:
771 DisableIR();
772 break;
773 }
774
775 BYTE buff [REPORT_LENGTH] = {0};
776 buff[0] = OUT_TYPE;
777 buff[1] = (continuous ? 0x04 : 0x00) | GetRumbleBit();
778 buff[2] = (BYTE)type;
779 WriteReport(buff);
780 // Sleep(15);
781 }
782 // ------------------------------------------------------------------------------------
783 void wiimote::SetLEDs (BYTE led_bits)
784 {
785 _ASSERT(IsConnected());
786 if(!IsConnected() || bInitInProgress)
787 return;
788
789 _ASSERT(led_bits <= 0x0f);
790 led_bits &= 0xf;
791
792 BYTE buff [REPORT_LENGTH] = {0};
793 buff[0] = OUT_LEDs;
794 buff[1] = (led_bits<<4) | GetRumbleBit();
795 WriteReport(buff);
796
797 Internal.LED.Bits = led_bits;
798 }
799 // ------------------------------------------------------------------------------------
800 void wiimote::SetRumble (bool on)
801 {
802 _ASSERT(IsConnected());
803 if(!IsConnected())
804 return;
805
806 if(Internal.bRumble == on)
807 return;
808
809 Internal.bRumble = on;
810
811 // if we're streaming audio, we don't need to send a report (sending it makes
812 // the audio glitch, and the rumble bit is sent with every report anyway)
813 if(IsPlayingAudio())
814 return;
815
816 BYTE buff [REPORT_LENGTH] = {0};
817 buff[0] = OUT_STATUS;
818 buff[1] = on? 0x01 : 0x00;
819 WriteReport(buff);
820 }
821 // ------------------------------------------------------------------------------------
822 unsigned __stdcall wiimote::AsyncRumbleThreadfunc (void* param)
823 {
824 // auto-disables rumble after x milliseconds:
825 _ASSERT(param);
826 wiimote &remote = *(wiimote*)param;
827
828 while(remote.IsConnected())
829 {
830 if(remote.AsyncRumbleTimeout)
831 {
832 DWORD current_time = timeGetTime();
833 if(current_time >= remote.AsyncRumbleTimeout)
834 {
835 if(remote.Internal.bRumble)
836 remote.SetRumble(false);
837 remote.AsyncRumbleTimeout = 0;
838 }
839 Sleep(1);
840 }
841 else
842 Sleep(4);
843 }
844 return 0;
845 }
846 // ------------------------------------------------------------------------------------
847 void wiimote::RumbleForAsync (unsigned milliseconds)
848 {
849 // rumble for a fixed amount of time
850 _ASSERT(IsConnected());
851 if(!IsConnected())
852 return;
853
854 SetRumble(true);
855
856 // show how long thread should wait to disable rumble again
857 // (it it's currently rumbling it will just extend the time)
858 AsyncRumbleTimeout = timeGetTime() + milliseconds;
859
860 // create the thread?
861 if(AsyncRumbleThread)
862 return;
863
864 AsyncRumbleThread = (HANDLE)_beginthreadex(NULL, 0, AsyncRumbleThreadfunc, this,
865 0, NULL);
866 _ASSERT(AsyncRumbleThread);
867 if(!AsyncRumbleThread) {
868 WARN(_T("couldn't create rumble thread!"));
869 return;
870 }
871 SetThreadPriority(AsyncRumbleThread, WORKER_THREAD_PRIORITY);
872 }
873 // ------------------------------------------------------------------------------------
874 void wiimote::RequestStatusReport ()
875 {
876 // (this can be called before we're fully connected)
877 _ASSERT(Handle != INVALID_HANDLE_VALUE);
878 if(Handle == INVALID_HANDLE_VALUE)
879 return;
880
881 BYTE buff [REPORT_LENGTH] = {0};
882 buff[0] = OUT_STATUS;
883 buff[1] = GetRumbleBit();
884 WriteReport(buff);
885 }
886 // ------------------------------------------------------------------------------------
887 bool wiimote::ReadAddress (int address, short size)
888 {
889 // asynchronous
890 BYTE buff [REPORT_LENGTH] = {0};
891 buff[0] = OUT_READMEMORY;
892 buff[1] = (BYTE)(((address & 0xff000000) >> 24) | GetRumbleBit());
893 buff[2] = (BYTE)( (address & 0x00ff0000) >> 16);
894 buff[3] = (BYTE)( (address & 0x0000ff00) >> 8);
895 buff[4] = (BYTE)( (address & 0x000000ff));
896 buff[5] = (BYTE)( (size & 0xff00 ) >> 8);
897 buff[6] = (BYTE)( (size & 0xff));
898 return WriteReport(buff);
899 }
900 // ------------------------------------------------------------------------------------
901 void wiimote::WriteData (int address, BYTE size, const BYTE* buff)
902 {
903 // asynchronous
904 BYTE write [REPORT_LENGTH] = {0};
905 write[0] = OUT_WRITEMEMORY;
906 write[1] = (BYTE)(((address & 0xff000000) >> 24) | GetRumbleBit());
907 write[2] = (BYTE)( (address & 0x00ff0000) >> 16);
908 write[3] = (BYTE)( (address & 0x0000ff00) >> 8);
909 write[4] = (BYTE)( (address & 0x000000ff));
910 write[5] = size;
911 memcpy(write+6, buff, size);
912 WriteReport(write);
913 }
914 // ------------------------------------------------------------------------------------
915 int wiimote::ParseInput (BYTE* buff)
916 {
917 int changed = 0;
918
919 // lock our internal state (so RefreshState() is blocked until we're done
920 EnterCriticalSection(&StateLock);
921
922 switch(buff[0])
923 {
924 case IN_BUTTONS:
925 DEEP_TRACE(_T(".. parsing buttons."));
926 changed |= ParseButtons(buff);
927 break;
928
929 case IN_BUTTONS_ACCEL:
930 DEEP_TRACE(_T(".. parsing buttons/accel."));
931 changed |= ParseButtons(buff);
932 if(!IsBalanceBoard())
933 changed |= ParseAccel(buff);
934 break;
935
936 case IN_BUTTONS_ACCEL_EXT:
937 DEEP_TRACE(_T(".. parsing extenion/accel."));
938 changed |= ParseButtons(buff);
939 changed |= ParseExtension(buff, 6);
940 if(!IsBalanceBoard())
941 changed |= ParseAccel(buff);
942 break;
943
944 case IN_BUTTONS_ACCEL_IR:
945 DEEP_TRACE(_T(".. parsing ir/accel."));
946 changed |= ParseButtons(buff);
947 if(!IsBalanceBoard()) {
948 changed |= ParseAccel(buff);
949 changed |= ParseIR(buff);
950 }
951 break;
952
953 case IN_BUTTONS_ACCEL_IR_EXT:
954 DEEP_TRACE(_T(".. parsing ir/extenion/accel."));
955 changed |= ParseButtons(buff);
956 changed |= ParseExtension(buff, 16);
957 if(!IsBalanceBoard()) {
958 changed |= ParseAccel(buff);
959 changed |= ParseIR (buff);
960 }
961 break;
962
963 case IN_BUTTONS_BALANCE_BOARD:
964 DEEP_TRACE(_T(".. parsing buttson/balance."));
965 changed |= ParseButtons(buff);
966 changed |= ParseExtension(buff, 3);
967 break;
968
969 case IN_READADDRESS:
970 DEEP_TRACE(_T(".. parsing read address."));
971 changed |= ParseButtons (buff);
972 changed |= ParseReadAddress(buff);
973 break;
974
975 case IN_STATUS:
976 DEEP_TRACE(_T(".. parsing status."));
977 changed |= ParseStatus(buff);
978 // show that we received the status report (used for output method
979 // detection during Connect())
980 bStatusReceived = true;
981 break;
982
983 default:
984 DEEP_TRACE(_T(".. ** unknown input ** (happens)."));
985 ///_ASSERT(0);
986 //Debug.WriteLine("Unknown report type: " + type.ToString());
987 LeaveCriticalSection(&StateLock);
988 return false;
989 }
990
991 // if we're recording and some state we care about has changed, insert it into
992 // the state history
993 if(Recording.bEnabled && (changed & Recording.TriggerFlags))
994 {
995 DEEP_TRACE(_T(".. adding state to history"));
996 state_event event;
997 event.time_ms = timeGetTime();
998 event.state = *(wiimote_state*)this;
999 Recording.StateHistory->push_back(event);
1000 }
1001
1002 // for polling: show which state has changed since the last RefreshState()
1003 InternalChanged = (state_change_flags)(InternalChanged | changed);
1004
1005 LeaveCriticalSection(&StateLock);
1006
1007 // callbacks: call it (if set & state the app is interested in has changed)
1008 if(changed & CallbackTriggerFlags)
1009 {
1010 DEEP_TRACE(_T(".. calling state change callback"));
1011 ChangedNotifier((state_change_flags)changed, Internal);
1012 if(ChangedCallback)
1013 ChangedCallback(*this, (state_change_flags)changed, Internal);
1014 }
1015
1016 DEEP_TRACE(_T(".. parse complete."));
1017 return true;
1018 }
1019 // ------------------------------------------------------------------------------------
1020 state_change_flags wiimote::RefreshState ()
1021 {
1022 // nothing changed since the last call?
1023 if(InternalChanged == NO_CHANGE)
1024 return NO_CHANGE;
1025
1026 // copy the internal state to our public data members:
1027 // synchronise the interal state with the read/parse thread (we don't want
1028 // values changing during the copy)
1029 EnterCriticalSection(&StateLock);
1030
1031 // remember which state changed since the last call
1032 state_change_flags changed = InternalChanged;
1033
1034 // preserve the application-set deadzones (if any)
1035 joystick::deadzone nunchuk_deadzone = Nunchuk.Joystick.DeadZone;
1036 joystick::deadzone classic_joyl_deadzone = ClassicController.JoystickL.DeadZone;
1037 joystick::deadzone classic_joyr_deadzone = ClassicController.JoystickR.DeadZone;
1038
1039 // copy the internal state to the public one
1040 *(wiimote_state*)this = Internal;
1041 InternalChanged = NO_CHANGE;
1042
1043 // restore the application-set deadzones
1044 Nunchuk.Joystick.DeadZone = nunchuk_deadzone;
1045 ClassicController.JoystickL.DeadZone = classic_joyl_deadzone;
1046 ClassicController.JoystickR.DeadZone = classic_joyr_deadzone;
1047
1048 LeaveCriticalSection(&StateLock);
1049
1050 return changed;
1051 }
1052 // ------------------------------------------------------------------------------------
1053 void wiimote::DetectMotionPlusExtensionAsync ()
1054 {
1055 #ifdef _DEBUG
1056 TRACE(_T("(looking for motion plus)"));
1057 #endif
1058 // show that we're expecting the result shortly
1059 MotionPlusDetectCount++;
1060 // MotionPLus reports at a different address than other extensions (until
1061 // activated, when it maps itself into the usual extension registers), so
1062 // try to detect it first:
1063 ReadAddress(REGISTER_MOTIONPLUS_DETECT, 6);
1064 }
1065 // ------------------------------------------------------------------------------------
1066 bool wiimote::EnableMotionPlus ()
1067 {
1068 _ASSERT(bMotionPlusDetected);
1069 if(!bMotionPlusDetected)
1070 return false;
1071 if(bMotionPlusEnabled)
1072 return true;
1073
1074 TRACE(_T("Enabling Motion Plus:"));
1075
1076 bMotionPlusExtension = false;
1077 bInitInProgress = true;
1078 bEnablingMotionPlus = true;
1079
1080 // Initialize it:
1081 WriteData(REGISTER_MOTIONPLUS_INIT , 0x55);
1082 // Sleep(50);
1083 // Enable it (this maps it to the standard extension port):
1084 WriteData(REGISTER_MOTIONPLUS_ENABLE, 0x04);
1085 // Sleep(50);
1086 Sleep(500);
1087 return true;
1088 }
1089 // ------------------------------------------------------------------------------------
1090 bool wiimote::DisableMotionPlus ()
1091 {
1092 if(!bMotionPlusDetected || !bMotionPlusEnabled)
1093 return false;
1094
1095 TRACE(_T("Disabling Motion Plus:"));
1096
1097 // disable it (this makes standard extensions visible again)
1098 WriteData(REGISTER_EXTENSION_INIT1, 0x55);
1099 return true;
1100 }
1101 // ------------------------------------------------------------------------------------
1102 void wiimote::InitializeExtension ()
1103 {
1104 TRACE(_T("Initialising Extension."));
1105 // wibrew.org: The new way to initialize the extension is by writing 0x55 to
1106 // 0x(4)A400F0, then writing 0x00 to 0x(4)A400FB. It works on all extensions, and
1107 // makes the extension type bytes unencrypted. This means that you no longer have
1108 // to decrypt the extension bytes using the transform listed above.
1109 bInitInProgress = true;
1110 _ASSERT(Internal.bExtension);
1111 // only initialize if it's not a MotionPlus
1112 if(!bEnablingMotionPlus) {
1113 WriteData (REGISTER_EXTENSION_INIT1, 0x55);
1114 WriteData (REGISTER_EXTENSION_INIT2, 0x00);
1115 }
1116 else
1117 bEnablingMotionPlus = false;
1118
1119 ReadAddress(REGISTER_EXTENSION_TYPE , 6);
1120 }
1121 // ------------------------------------------------------------------------------------
1122 int wiimote::ParseStatus (BYTE* buff)
1123 {
1124 // parse the buttons
1125 int changed = ParseButtons(buff);
1126
1127 // get the battery level
1128 BYTE battery_raw = buff[6];
1129 if(Internal.BatteryRaw != battery_raw)
1130 changed |= BATTERY_CHANGED;
1131 Internal.BatteryRaw = battery_raw;
1132 // it is estimated that ~200 is the maximum battery level
1133 Internal.BatteryPercent = battery_raw / 2;
1134
1135 // there is also a flag that shows if the battery is nearly empty
1136 bool drained = buff[3] & 0x01;
1137 if(drained != bBatteryDrained)
1138 {
1139 bBatteryDrained = drained;
1140 if(drained)
1141 changed |= BATTERY_DRAINED;
1142 }
1143
1144 // leds
1145 BYTE leds = buff[3] >> 4;
1146 if(leds != Internal.LED.Bits)
1147 changed |= LEDS_CHANGED;
1148 Internal.LED.Bits = leds;
1149
1150 // don't handle extensions until a connection is complete
1151 // if(bConnectInProgress)
1152 // return changed;
1153
1154 bool extension = ((buff[3] & 0x02) != 0);
1155 // TRACE(_T("(extension = %s)"), (extension? _T("TRUE") : _T("false")));
1156
1157 if(extension != Internal.bExtension)
1158 {
1159 if(!Internal.bExtension)
1160 {
1161 TRACE(_T("Extension connected:"));
1162 Internal.bExtension = true;
1163 InitializeExtension();
1164 }
1165 else{
1166 TRACE(_T("Extension disconnected."));
1167 Internal.bExtension = false;
1168 Internal.ExtensionType = wiimote_state::NONE;
1169 bMotionPlusEnabled = false;
1170 bMotionPlusExtension = false;
1171 bMotionPlusDetected = false;
1172 bInitInProgress = false;
1173 bEnablingMotionPlus = false;
1174 changed |= EXTENSION_DISCONNECTED;
1175 // renable reports
1176 // SetReportType(ReportType);
1177 }
1178 }
1179
1180 return changed;
1181 }
1182 // ------------------------------------------------------------------------------------
1183 int wiimote::ParseButtons (BYTE* buff)
1184 {
1185 int changed = 0;
1186
1187 // WORD bits = *(WORD*)(buff+1);
1188 WORD bits = *(WORD*)(buff+1) & Button.ALL;
1189
1190 if(bits != Internal.Button.Bits)
1191 changed |= BUTTONS_CHANGED;
1192 Internal.Button.Bits = bits;
1193
1194 return changed;
1195 }
1196 // ------------------------------------------------------------------------------------
1197 bool wiimote::EstimateOrientationFrom (wiimote_state::acceleration &accel)
1198 {
1199 // Orientation estimate from acceleration data (shared between wiimote and nunchuk)
1200 // return true if the orientation was updated
1201
1202 // assume the controller is stationary if the acceleration vector is near
1203 // 1g for several updates (this may not always be correct)
1204 float length_sq = square(accel.X) + square(accel.Y) + square(accel.Z);
1205
1206 // TODO: as I'm comparing _squared_ length, I really need different
1207 // min/max epsilons...
1208 #define DOT(x1,y1,z1, x2,y2,z2) ((x1*x2) + (y1*y2) + (z1*z2))
1209
1210 static const float epsilon = 0.2f;
1211 if((length_sq >= (1.f-epsilon)) && (length_sq <= (1.f+epsilon)))
1212 {
1213 if(++WiimoteNearGUpdates < 2)
1214 return false;
1215
1216 // wiimote seems to be stationary: normalize the current acceleration
1217 // (ie. the assumed gravity vector)
1218 float inv_len = 1.f / sqrt(length_sq);
1219 float x = accel.X * inv_len;
1220 float y = accel.Y * inv_len;
1221 float z = accel.Z * inv_len;
1222
1223 // copy the values
1224 accel.Orientation.X = x;
1225 accel.Orientation.Y = y;
1226 accel.Orientation.Z = z;
1227
1228 // and extract pitch & roll from them:
1229 // (may not be optimal)
1230 float pitch = -asin(y) * 57.2957795f;
1231 // float roll = asin(x) * 57.2957795f;
1232 float roll = atan2(x,z) * 57.2957795f;
1233 if(z < 0) {
1234 pitch = (y < 0)? 180 - pitch : -180 - pitch;
1235 roll = (x < 0)? -180 - roll : 180 - roll;
1236 }
1237
1238 accel.Orientation.Pitch = pitch;
1239 accel.Orientation.Roll = roll;
1240
1241 // show that we just updated orientation
1242 accel.Orientation.UpdateAge = 0;
1243 #ifdef BEEP_ON_ORIENTATION_ESTIMATE
1244 Beep(2000, 1);
1245 #endif
1246 return true; // updated
1247 }
1248
1249 // not updated this time:
1250 WiimoteNearGUpdates = 0;
1251 // age the last orientation update
1252 accel.Orientation.UpdateAge++;
1253 return false;
1254 }
1255 // ------------------------------------------------------------------------------------
1256 void wiimote::ApplyJoystickDeadZones (wiimote_state::joystick &joy)
1257 {
1258 // apply the deadzones to each axis (if set)
1259 if((joy.DeadZone.X > 0.f) && (joy.DeadZone.X <= 1.f))
1260 {
1261 if(fabs(joy.X) <= joy.DeadZone.X)
1262 joy.X = 0;
1263 else{
1264 joy.X -= joy.DeadZone.X * sign(joy.X);
1265 joy.X /= 1.f - joy.DeadZone.X;
1266 }
1267 }
1268 if((joy.DeadZone.Y > 0.f) && (joy.DeadZone.Y <= 1.f))
1269 {
1270 if(fabs(joy.Y) <= joy.DeadZone.Y)
1271 joy.Y = 0;
1272 else{
1273 joy.Y -= joy.DeadZone.Y * sign(joy.Y);
1274 joy.Y /= 1.f - joy.DeadZone.Y;
1275 }
1276 }
1277 }
1278 // ------------------------------------------------------------------------------------
1279 int wiimote::ParseAccel (BYTE* buff)
1280 {
1281 int changed = 0;
1282
1283 BYTE raw_x = buff[3];
1284 BYTE raw_y = buff[4];
1285 BYTE raw_z = buff[5];
1286
1287 if((raw_x != Internal.Acceleration.RawX) ||
1288 (raw_y != Internal.Acceleration.RawY) ||
1289 (raw_z != Internal.Acceleration.RawZ))
1290 changed |= ACCEL_CHANGED;
1291
1292 Internal.Acceleration.RawX = raw_x;
1293 Internal.Acceleration.RawY = raw_y;
1294 Internal.Acceleration.RawZ = raw_z;
1295
1296 // avoid / 0.0 when calibration data hasn't arrived yet
1297 if(Internal.CalibrationInfo.X0)
1298 {
1299 Internal.Acceleration.X =
1300 ((float)Internal.Acceleration.RawX - Internal.CalibrationInfo.X0) /
1301 ((float)Internal.CalibrationInfo.XG - Internal.CalibrationInfo.X0);
1302 Internal.Acceleration.Y =
1303 ((float)Internal.Acceleration.RawY - Internal.CalibrationInfo.Y0) /
1304 ((float)Internal.CalibrationInfo.YG - Internal.CalibrationInfo.Y0);
1305 Internal.Acceleration.Z =
1306 ((float)Internal.Acceleration.RawZ - Internal.CalibrationInfo.Z0) /
1307 ((float)Internal.CalibrationInfo.ZG - Internal.CalibrationInfo.Z0);
1308 }
1309 else{
1310 Internal.Acceleration.X =
1311 Internal.Acceleration.Y =
1312 Internal.Acceleration.Z = 0.f;
1313 }
1314
1315 // see if we can estimate the orientation from the current values
1316 if(EstimateOrientationFrom(Internal.Acceleration))
1317 changed |= ORIENTATION_CHANGED;
1318
1319 return changed;
1320 }
1321 // ------------------------------------------------------------------------------------
1322 int wiimote::ParseIR (BYTE* buff)
1323 {
1324 if(Internal.IR.Mode == wiimote_state::ir::OFF)
1325 return NO_CHANGE;
1326
1327 // avoid garbage values when the MotionPlus is enabled, but the app is
1328 // still using the extended IR report type
1329 if(bMotionPlusEnabled && (Internal.IR.Mode == wiimote_state::ir::EXTENDED))
1330 return NO_CHANGE;
1331
1332 // take a copy of the existing IR state (so we can detect changes)
1333 wiimote_state::ir prev_ir = Internal.IR;
1334
1335 // only updates the other values if the dots are visible (so that the last
1336 // valid values stay unmodified)
1337 switch(Internal.IR.Mode)
1338 {
1339 case wiimote_state::ir::BASIC:
1340 // 2 dots are encoded in 5 bytes, so read 2 at a time
1341 for(unsigned step=0; step<2; step++)
1342 {
1343 ir::dot &dot0 = Internal.IR.Dot[step*2 ];
1344 ir::dot &dot1 = Internal.IR.Dot[step*2+1];
1345 const unsigned offs = 6 + (step*5); // 5 bytes for 2 dots
1346
1347 dot0.bVisible = !(buff[offs ] == 0xff && buff[offs+1] == 0xff);
1348 dot1.bVisible = !(buff[offs+3] == 0xff && buff[offs+4] == 0xff);
1349
1350 if(dot0.bVisible) {
1351 dot0.RawX = buff[offs ] | ((buff[offs+2] >> 4) & 0x03) << 8;;
1352 dot0.RawY = buff[offs+1] | ((buff[offs+2] >> 6) & 0x03) << 8;;
1353 dot0.X = 1.f - (dot0.RawX / (float)wiimote_state::ir::MAX_RAW_X);
1354 dot0.Y = (dot0.RawY / (float)wiimote_state::ir::MAX_RAW_Y);
1355 }
1356 if(dot1.bVisible) {
1357 dot1.RawX = buff[offs+3] | ((buff[offs+2] >> 0) & 0x03) << 8;
1358 dot1.RawY = buff[offs+4] | ((buff[offs+2] >> 2) & 0x03) << 8;
1359 dot1.X = 1.f - (dot1.RawX / (float)wiimote_state::ir::MAX_RAW_X);
1360 dot1.Y = (dot1.RawY / (float)wiimote_state::ir::MAX_RAW_Y);
1361 }
1362 }
1363 break;
1364
1365 case wiimote_state::ir::EXTENDED:
1366 // each dot is encoded into 3 bytes
1367 for(unsigned index=0; index<4; index++)
1368 {
1369 ir::dot &dot = Internal.IR.Dot[index];
1370 const unsigned offs = 6 + (index * 3);
1371
1372 dot.bVisible = !(buff[offs ]==0xff && buff[offs+1]==0xff &&
1373 buff[offs+2]==0xff);
1374 if(dot.bVisible) {
1375 dot.RawX = buff[offs ] | ((buff[offs+2] >> 4) & 0x03) << 8;
1376 dot.RawY = buff[offs+1] | ((buff[offs+2] >> 6) & 0x03) << 8;
1377 dot.X = 1.f - (dot.RawX / (float)wiimote_state::ir::MAX_RAW_X);
1378 dot.Y = (dot.RawY / (float)wiimote_state::ir::MAX_RAW_Y);
1379 dot.Size = buff[offs+2] & 0x0f;
1380 }
1381 }
1382 break;
1383
1384 case wiimote_state::ir::FULL:
1385 _ASSERT(0); // not supported yet;
1386 break;
1387 }
1388
1389 return memcmp(&prev_ir, &Internal.IR, sizeof(Internal.IR))? IR_CHANGED : 0;
1390 }
1391 // ------------------------------------------------------------------------------------
1392 inline float wiimote::GetBalanceValue (short sensor, short min, short mid, short max)
1393 {
1394 if(max == mid || mid == min)
1395 return 0;
1396
1397 float val = (sensor < mid)?
1398 68.0f * ((float)(sensor - min) / (mid - min)) :
1399 68.0f * ((float)(sensor - mid) / (max - mid)) + 68.0f;
1400
1401 // divide by four (so that each sensor is correct)
1402 return val * 0.25f;
1403 }
1404 // ------------------------------------------------------------------------------------
1405 int wiimote::ParseExtension (BYTE *buff, unsigned offset)
1406 {
1407 int changed = 0;
1408
1409 switch(Internal.ExtensionType)
1410 {
1411 case wiimote_state::NUNCHUK:
1412 {
1413 // buttons
1414 bool c = (buff[offset+5] & 0x02) == 0;
1415 bool z = (buff[offset+5] & 0x01) == 0;
1416
1417 if((c != Internal.Nunchuk.C) || (z != Internal.Nunchuk.Z))
1418 changed |= NUNCHUK_BUTTONS_CHANGED;
1419
1420 Internal.Nunchuk.C = c;
1421 Internal.Nunchuk.Z = z;
1422
1423 // acceleration
1424 {
1425 wiimote_state::acceleration &accel = Internal.Nunchuk.Acceleration;
1426
1427 BYTE raw_x = buff[offset+2];
1428 BYTE raw_y = buff[offset+3];
1429 BYTE raw_z = buff[offset+4];
1430 if((raw_x != accel.RawX) || (raw_y != accel.RawY) || (raw_z != accel.RawZ))
1431 changed |= NUNCHUK_ACCEL_CHANGED;
1432
1433 accel.RawX = raw_x;
1434 accel.RawY = raw_y;
1435 accel.RawZ = raw_z;
1436
1437 wiimote_state::nunchuk::calibration_info &calib =
1438 Internal.Nunchuk.CalibrationInfo;
1439 accel.X = ((float)raw_x - calib.X0) / ((float)calib.XG - calib.X0);
1440 accel.Y = ((float)raw_y - calib.Y0) / ((float)calib.YG - calib.Y0);
1441 accel.Z = ((float)raw_z - calib.Z0) / ((float)calib.ZG - calib.Z0);
1442
1443 // try to extract orientation from the accel:
1444 if(EstimateOrientationFrom(accel))
1445 changed |= NUNCHUK_ORIENTATION_CHANGED;
1446 }
1447 {
1448 // joystick:
1449 wiimote_state::joystick &joy = Internal.Nunchuk.Joystick;
1450
1451 float raw_x = buff[offset+0];
1452 float raw_y = buff[offset+1];
1453
1454 if((raw_x != joy.RawX) || (raw_y != joy.RawY))
1455 changed |= NUNCHUK_JOYSTICK_CHANGED;
1456
1457 joy.RawX = raw_x;
1458 joy.RawY = raw_y;
1459
1460 // apply the calibration data
1461 wiimote_state::nunchuk::calibration_info &calib =
1462 Internal.Nunchuk.CalibrationInfo;
1463 if(Internal.Nunchuk.CalibrationInfo.MaxX != 0x00)
1464 joy.X = ((float)raw_x - calib.MidX) / ((float)calib.MaxX - calib.MinX);
1465 if(calib.MaxY != 0x00)
1466 joy.Y = ((float)raw_y - calib.MidY) / ((float)calib.MaxY - calib.MinY);
1467
1468 // i prefer the outputs to range -1 - +1 (note this also affects the
1469 // deadzone calculations)
1470 joy.X *= 2; joy.Y *= 2;
1471
1472 // apply the public deadzones to the internal state (if set)
1473 joy.DeadZone = Nunchuk.Joystick.DeadZone;
1474 ApplyJoystickDeadZones(joy);
1475 }
1476 }
1477 break;
1478
1479 case wiimote_state::CLASSIC:
1480 case wiimote_state::GH3_GHWT_GUITAR:
1481 case wiimote_state::GHWT_DRUMS:
1482 {
1483 // buttons:
1484 WORD bits = *(WORD*)(buff+offset+4);
1485 bits = ~bits; // need to invert bits since 0 is down, and 1 is up
1486
1487 if(bits != Internal.ClassicController.Button.Bits)
1488 changed |= CLASSIC_BUTTONS_CHANGED;
1489
1490 Internal.ClassicController.Button.Bits = bits;
1491
1492 // joysticks:
1493 wiimote_state::joystick &joyL = Internal.ClassicController.JoystickL;
1494 wiimote_state::joystick &joyR = Internal.ClassicController.JoystickR;
1495
1496 float l_raw_x = (float) (buff[offset+0] & 0x3f);
1497 float l_raw_y = (float) (buff[offset+1] & 0x3f);
1498 float r_raw_x = (float)((buff[offset+2] >> 7) |
1499 ((buff[offset+1] & 0xc0) >> 5) |
1500 ((buff[offset+0] & 0xc0) >> 3));
1501 float r_raw_y = (float) (buff[offset+2] & 0x1f);
1502
1503 if((joyL.RawX != l_raw_x) || (joyL.RawY != l_raw_y))
1504 changed |= CLASSIC_JOYSTICK_L_CHANGED;
1505 if((joyR.RawX != r_raw_x) || (joyR.RawY != r_raw_y))
1506 changed |= CLASSIC_JOYSTICK_R_CHANGED;
1507
1508 joyL.RawX = l_raw_x; joyL.RawY = l_raw_y;
1509 joyR.RawX = r_raw_x; joyR.RawY = r_raw_y;
1510
1511 // apply calibration
1512 wiimote_state::classic_controller::calibration_info &calib =
1513 Internal.ClassicController.CalibrationInfo;
1514 if(calib.MaxXL != 0x00)
1515 joyL.X = (joyL.RawX - calib.MidXL) / ((float)calib.MaxXL - calib.MinXL);
1516 if(calib.MaxYL != 0x00)
1517 joyL.Y = (joyL.RawY - calib.MidYL) / ((float)calib.MaxYL - calib.MinYL);
1518 if(calib.MaxXR != 0x00)
1519 joyR.X = (joyR.RawX - calib.MidXR) / ((float)calib.MaxXR - calib.MinXR);
1520 if(calib.MaxYR != 0x00)
1521 joyR.Y = (joyR.RawY - calib.MidYR) / ((float)calib.MaxYR - calib.MinYR);
1522
1523 // i prefer the joystick outputs to range -1 - +1 (note this also affects
1524 // the deadzone calculations)
1525 joyL.X *= 2; joyL.Y *= 2; joyR.X *= 2; joyR.Y *= 2;
1526
1527 // apply the public deadzones to the internal state (if set)
1528 joyL.DeadZone = ClassicController.JoystickL.DeadZone;
1529 joyR.DeadZone = ClassicController.JoystickR.DeadZone;
1530 ApplyJoystickDeadZones(joyL);
1531 ApplyJoystickDeadZones(joyR);
1532
1533 // triggers
1534 BYTE raw_trigger_l = ((buff[offset+2] & 0x60) >> 2) |
1535 (buff[offset+3] >> 5);
1536 BYTE raw_trigger_r = buff[offset+3] & 0x1f;
1537
1538 if((raw_trigger_l != Internal.ClassicController.RawTriggerL) ||
1539 (raw_trigger_r != Internal.ClassicController.RawTriggerR))
1540 changed |= CLASSIC_TRIGGERS_CHANGED;
1541
1542 Internal.ClassicController.RawTriggerL = raw_trigger_l;
1543 Internal.ClassicController.RawTriggerR = raw_trigger_r;
1544
1545 if(calib.MaxTriggerL != 0x00)
1546 Internal.ClassicController.TriggerL =
1547 (float)Internal.ClassicController.RawTriggerL /
1548 ((float)calib.MaxTriggerL - calib.MinTriggerL);
1549 if(calib.MaxTriggerR != 0x00)
1550 Internal.ClassicController.TriggerR =
1551 (float)Internal.ClassicController.RawTriggerR /
1552 ((float)calib.MaxTriggerR - calib.MinTriggerR);
1553 }
1554 break;
1555
1556 case BALANCE_BOARD:
1557 {
1558 wiimote_state::balance_board::sensors_raw prev_raw =
1559 Internal.BalanceBoard.Raw;
1560 Internal.BalanceBoard.Raw.TopR =
1561 (short)((short)buff[offset+0] << 8 | buff[offset+1]);
1562 Internal.BalanceBoard.Raw.BottomR =
1563 (short)((short)buff[offset+2] << 8 | buff[offset+3]);
1564 Internal.BalanceBoard.Raw.TopL =
1565 (short)((short)buff[offset+4] << 8 | buff[offset+5]);
1566 Internal.BalanceBoard.Raw.BottomL =
1567 (short)((short)buff[offset+6] << 8 | buff[offset+7]);
1568
1569 if((Internal.BalanceBoard.Raw.TopL != prev_raw.TopL) ||
1570 (Internal.BalanceBoard.Raw.TopR != prev_raw.TopR) ||
1571 (Internal.BalanceBoard.Raw.BottomL != prev_raw.BottomL) ||
1572 (Internal.BalanceBoard.Raw.BottomR != prev_raw.BottomR))
1573 changed |= BALANCE_WEIGHT_CHANGED;
1574
1575 Internal.BalanceBoard.Kg.TopL =
1576 GetBalanceValue(Internal.BalanceBoard.Raw.TopL,
1577 Internal.BalanceBoard.CalibrationInfo.Kg0 .TopL,
1578 Internal.BalanceBoard.CalibrationInfo.Kg17.TopL,
1579 Internal.BalanceBoard.CalibrationInfo.Kg34.TopL);
1580 Internal.BalanceBoard.Kg.TopR =
1581 GetBalanceValue(Internal.BalanceBoard.Raw.TopR,
1582 Internal.BalanceBoard.CalibrationInfo.Kg0 .TopR,
1583 Internal.BalanceBoard.CalibrationInfo.Kg17.TopR,
1584 Internal.BalanceBoard.CalibrationInfo.Kg34.TopR);
1585 Internal.BalanceBoard.Kg.BottomL =
1586 GetBalanceValue(Internal.BalanceBoard.Raw.BottomL,
1587 Internal.BalanceBoard.CalibrationInfo.Kg0 .BottomL,
1588 Internal.BalanceBoard.CalibrationInfo.Kg17.BottomL,
1589 Internal.BalanceBoard.CalibrationInfo.Kg34.BottomL);
1590 Internal.BalanceBoard.Kg.BottomR =
1591 GetBalanceValue(Internal.BalanceBoard.Raw.BottomR,
1592 Internal.BalanceBoard.CalibrationInfo.Kg0 .BottomR,
1593 Internal.BalanceBoard.CalibrationInfo.Kg17.BottomR,
1594 Internal.BalanceBoard.CalibrationInfo.Kg34.BottomR);
1595
1596 // uses these as the 'at rest' offsets? (immediately after Connect(),
1597 // or if the app called CalibrateAtRest())
1598 if(bCalibrateAtRest) {
1599 bCalibrateAtRest = false;
1600 TRACE(_T(".. Auto-removing 'at rest' BBoard offsets."));
1601 Internal.BalanceBoard.AtRestKg = Internal.BalanceBoard.Kg;
1602 }
1603
1604 // remove the 'at rest' offsets
1605 Internal.BalanceBoard.Kg.TopL -= BalanceBoard.AtRestKg.TopL;
1606 Internal.BalanceBoard.Kg.TopR -= BalanceBoard.AtRestKg.TopR;
1607 Internal.BalanceBoard.Kg.BottomL -= BalanceBoard.AtRestKg.BottomL;
1608 Internal.BalanceBoard.Kg.BottomR -= BalanceBoard.AtRestKg.BottomR;
1609
1610 // compute the average
1611 Internal.BalanceBoard.Kg.Total = Internal.BalanceBoard.Kg.TopL +
1612 Internal.BalanceBoard.Kg.TopR +
1613 Internal.BalanceBoard.Kg.BottomL +
1614 Internal.BalanceBoard.Kg.BottomR;
1615 // and convert to Lbs
1616 const float KG2LB = 2.20462262f;
1617 Internal.BalanceBoard.Lb = Internal.BalanceBoard.Kg;
1618 Internal.BalanceBoard.Lb.TopL *= KG2LB;
1619 Internal.BalanceBoard.Lb.TopR *= KG2LB;
1620 Internal.BalanceBoard.Lb.BottomL *= KG2LB;
1621 Internal.BalanceBoard.Lb.BottomR *= KG2LB;
1622 Internal.BalanceBoard.Lb.Total *= KG2LB;
1623 }
1624 break;
1625
1626 case MOTION_PLUS:
1627 {
1628 bMotionPlusDetected = true;
1629 bMotionPlusEnabled = true;
1630
1631 short yaw = ((unsigned short)buff[offset+3] & 0xFC)<<6 |
1632 (unsigned short)buff[offset+0];
1633 short pitch = ((unsigned short)buff[offset+5] & 0xFC)<<6 |
1634 (unsigned short)buff[offset+2];
1635 short roll = ((unsigned short)buff[offset+4] & 0xFC)<<6 |
1636 (unsigned short)buff[offset+1];
1637
1638 // we get one set of bogus values when the MotionPlus is disconnected,
1639 // so ignore them
1640 if((yaw != 0x3fff) || (pitch != 0x3fff) || (roll != 0x3fff))
1641 {
1642 wiimote_state::motion_plus::sensors_raw &raw = Internal.MotionPlus.Raw;
1643
1644 if((raw.Yaw != yaw) || (raw.Pitch != pitch) || (raw.Roll != roll))
1645 changed |= MOTIONPLUS_SPEED_CHANGED;
1646
1647 raw.Yaw = yaw;
1648 raw.Pitch = pitch;
1649 raw.Roll = roll;
1650
1651 // convert to float values
1652 bool yaw_slow = (buff[offset+3] & 0x2) == 0x2;
1653 bool pitch_slow = (buff[offset+3] & 0x1) == 0x1;
1654 bool roll_slow = (buff[offset+4] & 0x2) == 0x2;
1655 float y_scale = yaw_slow? 0.05f : 0.25f;
1656 float p_scale = pitch_slow? 0.05f : 0.25f;
1657 float r_scale = roll_slow? 0.05f : 0.25f;
1658
1659 Internal.MotionPlus.Speed.Yaw = -(raw.Yaw - 0x1F7F) * y_scale;
1660 Internal.MotionPlus.Speed.Pitch = -(raw.Pitch - 0x1F7F) * p_scale;
1661 Internal.MotionPlus.Speed.Roll = -(raw.Roll - 0x1F7F) * r_scale;
1662
1663 // show if there's an extension plugged into the MotionPlus:
1664 bool extension = buff[offset+4] & 1;
1665 if(extension != bMotionPlusExtension)
1666 {
1667 if(extension) {
1668 TRACE(_T(".. MotionPlus extension found."));
1669 changed |= MOTIONPLUS_EXTENSION_CONNECTED;
1670 }
1671 else{
1672 TRACE(_T(".. MotionPlus' extension disconnected."));
1673 changed |= MOTIONPLUS_EXTENSION_DISCONNECTED;
1674 }
1675 }
1676 bMotionPlusExtension = extension;
1677 }
1678 // while we're getting data, the plus is obviously detected/enabled
1679 // bMotionPlusDetected = bMotionPlusEnabled = true;
1680 }
1681 break;
1682 }
1683
1684 return changed;
1685 }
1686 // ------------------------------------------------------------------------------------
1687 int wiimote::ParseReadAddress (BYTE* buff)
1688 {
1689 // decode the address that was queried:
1690 int address = buff[4]<<8 | buff[5];
1691 int size = buff[3] >> 4;
1692 int changed = 0;
1693
1694 if((buff[3] & 0x08) != 0) {
1695 WARN(_T("error: read address not valid."));
1696 _ASSERT(0);
1697 return NO_CHANGE;
1698 }
1699 // address read failed (write-only)?
1700 else if((buff[3] & 0x07) != 0)
1701 {
1702 // this also happens when attempting to detect a non-existant MotionPlus
1703 if(MotionPlusDetectCount)
1704 {
1705 --MotionPlusDetectCount;
1706 if(Internal.ExtensionType == MOTION_PLUS)
1707 {
1708 if(bMotionPlusDetected)
1709 TRACE(_T(".. MotionPlus removed."));
1710 bMotionPlusDetected = false;
1711 bMotionPlusEnabled = false;
1712 // the MotionPlus can sometimes get confused - initializing
1713 // extenions fixes it:
1714 // if(address == 0xfa)
1715 // InitializeExtension();
1716 }
1717 }
1718 else
1719 WARN(_T("error: attempt to read from write-only register 0x%X."), buff[3]);
1720
1721 return NO_CHANGE;
1722 }
1723
1724 // *NOTE*: this is a major (but convenient) hack! The returned data only
1725 // contains the lower two bytes of the address that was queried.
1726 // as these don't collide between any of the addresses/registers
1727 // we currently read, it's OK to match just those two bytes
1728
1729 // skip the header
1730 buff += 6;
1731
1732 switch(address)
1733 {
1734 case (REGISTER_CALIBRATION & 0xffff):
1735 {
1736 _ASSERT(size == 6);
1737 TRACE(_T(".. got wiimote calibration."));
1738 Internal.CalibrationInfo.X0 = buff[0];
1739 Internal.CalibrationInfo.Y0 = buff[1];
1740 Internal.CalibrationInfo.Z0 = buff[2];
1741 Internal.CalibrationInfo.XG = buff[4];
1742 Internal.CalibrationInfo.YG = buff[5];
1743 Internal.CalibrationInfo.ZG = buff[6];
1744 //changed |= CALIBRATION_CHANGED;
1745 }
1746 break;
1747
1748 // note: this covers both the normal extension and motion plus extension
1749 // addresses (0x4a400fa / 0x4a600fa)
1750 case (REGISTER_EXTENSION_TYPE & 0xffff):
1751 {
1752 _ASSERT(size == 5);
1753 QWORD type = *(QWORD*)buff;
1754
1755 // TRACE(_T("(found extension 0x%I64x)"), type);
1756
1757 static const QWORD NUNCHUK = 0x000020A40000ULL;
1758 static const QWORD CLASSIC = 0x010120A40000ULL;
1759 static const QWORD GH3_GHWT_GUITAR = 0x030120A40000ULL;
1760 static const QWORD GHWT_DRUMS = 0x030120A40001ULL;
1761 static const QWORD BALANCE_BOARD = 0x020420A40000ULL;
1762 static const QWORD MOTION_PLUS = 0x050420A40000ULL;
1763 static const QWORD MOTION_PLUS_DETECT = 0x050020a60000ULL;
1764 static const QWORD MOTION_PLUS_DETECT2 = 0x050420a60000ULL;
1765 static const QWORD PARTIALLY_INSERTED = 0xffffffffffffULL;
1766
1767 // MotionPlus: _before_ it's been activated
1768 if((type == MOTION_PLUS_DETECT) || (type == MOTION_PLUS_DETECT2))
1769 {
1770 if(!bMotionPlusDetected) {
1771 TRACE(_T("Motion Plus detected!"));
1772 changed |= MOTIONPLUS_DETECTED;
1773 }
1774 bMotionPlusDetected = true;
1775 --MotionPlusDetectCount;
1776 break;
1777 }
1778
1779 #define IF_TYPE(id) if(type == id) { \
1780 /* sometimes it comes in more than once */ \
1781 if(Internal.ExtensionType == wiimote_state::id)\
1782 break; \
1783 Internal.ExtensionType = wiimote_state::id;
1784
1785 // MotionPlus: once it's activated & mapped to the standard ext. port
1786 IF_TYPE(MOTION_PLUS)
1787 TRACE(_T(".. Motion Plus!"));
1788 // and start a query for the calibration data
1789 ReadAddress(REGISTER_EXTENSION_CALIBRATION, 16);
1790 bMotionPlusDetected = true;
1791 }
1792 else IF_TYPE(NUNCHUK)
1793 TRACE(_T(".. Nunchuk!"));
1794 bMotionPlusEnabled = false;
1795 // and start a query for the calibration data
1796 ReadAddress(REGISTER_EXTENSION_CALIBRATION, 16);
1797 }
1798 else IF_TYPE(CLASSIC)
1799 TRACE(_T(".. Classic Controller!"));
1800 bMotionPlusEnabled = false;
1801 // and start a query for the calibration data
1802 ReadAddress(REGISTER_EXTENSION_CALIBRATION, 16);
1803 }
1804 else IF_TYPE(GH3_GHWT_GUITAR)
1805 // sometimes it comes in more than once?
1806 TRACE(_T(".. GH3/GHWT Guitar Controller!"));
1807 bMotionPlusEnabled = false;
1808 // and start a query for the calibration data
1809 ReadAddress(REGISTER_EXTENSION_CALIBRATION, 16);
1810 }
1811 else IF_TYPE(GHWT_DRUMS)
1812 TRACE(_T(".. GHWT Drums!"));
1813 bMotionPlusEnabled = false;
1814 // and start a query for the calibration data
1815 ReadAddress(REGISTER_EXTENSION_CALIBRATION, 16);
1816 }
1817 else IF_TYPE(BALANCE_BOARD)
1818 TRACE(_T(".. Balance Board!"));
1819 bMotionPlusEnabled = false;
1820 // and start a query for the calibration data
1821 ReadAddress(REGISTER_BALANCE_CALIBRATION, 24);
1822 }
1823 else if(type == PARTIALLY_INSERTED) {
1824 // sometimes it comes in more than once?
1825 if(Internal.ExtensionType == wiimote_state::PARTIALLY_INSERTED)
1826 Sleep(50);
1827 TRACE(_T(".. partially inserted!"));
1828 bMotionPlusEnabled = false;
1829 Internal.ExtensionType = wiimote_state::PARTIALLY_INSERTED;
1830 changed |= EXTENSION_PARTIALLY_INSERTED;
1831 // try initializing the extension again by requesting another
1832 // status report (this usually fixes it)
1833 Internal.bExtension = false;
1834 RequestStatusReport();
1835 }
1836 else{
1837 TRACE(_T("unknown extension controller found (0x%I64x)"), type);
1838 }
1839 }
1840 break;
1841
1842 case (REGISTER_EXTENSION_CALIBRATION & 0xffff):
1843 case (REGISTER_BALANCE_CALIBRATION & 0xffff):
1844 {
1845 // _ASSERT(((Internal.ExtensionType == BALANCE_BOARD) && (size == 31)) ||
1846 // ((Internal.ExtensionType != BALANCE_BOARD) && (size == 15)));
1847
1848 switch(Internal.ExtensionType)
1849 {
1850 case wiimote_state::NUNCHUK:
1851 {
1852 wiimote_state::nunchuk::calibration_info
1853 &calib = Internal.Nunchuk.CalibrationInfo;
1854
1855 calib.X0 = buff[ 0];
1856 calib.Y0 = buff[ 1];
1857 calib.Z0 = buff[ 2];
1858 calib.XG = buff[ 4];
1859 calib.YG = buff[ 5];
1860 calib.ZG = buff[ 6];
1861 calib.MaxX = buff[ 8];
1862 calib.MinX = buff[ 9];
1863 calib.MidX = buff[10];
1864 calib.MaxY = buff[11];
1865 calib.MinY = buff[12];
1866 calib.MidY = buff[13];
1867
1868 changed |= NUNCHUK_CONNECTED;//|NUNCHUK_CALIBRATION_CHANGED;
1869 // reenable reports
1870 // SetReportType(ReportType);
1871 }
1872 break;
1873
1874 case wiimote_state::CLASSIC:
1875 case wiimote_state::GH3_GHWT_GUITAR:
1876 case wiimote_state::GHWT_DRUMS:
1877 {
1878 wiimote_state::classic_controller::calibration_info
1879 &calib = Internal.ClassicController.CalibrationInfo;
1880
1881 calib.MaxXL = buff[ 0] >> 2;
1882 calib.MinXL = buff[ 1] >> 2;
1883 calib.MidXL = buff[ 2] >> 2;
1884 calib.MaxYL = buff[ 3] >> 2;
1885 calib.MinYL = buff[ 4] >> 2;
1886 calib.MidYL = buff[ 5] >> 2;
1887 calib.MaxXR = buff[ 6] >> 3;
1888 calib.MinXR = buff[ 7] >> 3;
1889 calib.MidXR = buff[ 8] >> 3;
1890 calib.MaxYR = buff[ 9] >> 3;
1891 calib.MinYR = buff[10] >> 3;
1892 calib.MidYR = buff[11] >> 3;
1893 // this doesn't seem right...
1894 // calib.MinTriggerL = buff[12] >> 3;
1895 // calib.MaxTriggerL = buff[14] >> 3;
1896 // calib.MinTriggerR = buff[13] >> 3;
1897 // calib.MaxTriggerR = buff[15] >> 3;
1898 calib.MinTriggerL = 0;
1899 calib.MaxTriggerL = 31;
1900 calib.MinTriggerR = 0;
1901 calib.MaxTriggerR = 31;
1902
1903 changed |= CLASSIC_CONNECTED;//|CLASSIC_CALIBRATION_CHANGED;
1904 // reenable reports
1905 // SetReportType(ReportType);
1906 }
1907 break;
1908
1909 case BALANCE_BOARD:
1910 {
1911 // first part, 0 & 17kg calibration values
1912 wiimote_state::balance_board::calibration_info
1913 &calib = Internal.BalanceBoard.CalibrationInfo;
1914
1915 calib.Kg0 .TopR = (short)((short)buff[0] << 8 | buff[1]);
1916 calib.Kg0 .BottomR = (short)((short)buff[2] << 8 | buff[3]);
1917 calib.Kg0 .TopL = (short)((short)buff[4] << 8 | buff[5]);
1918 calib.Kg0 .BottomL = (short)((short)buff[6] << 8 | buff[7]);
1919
1920 calib.Kg17.TopR = (short)((short)buff[8] << 8 | buff[9]);
1921 calib.Kg17.BottomR = (short)((short)buff[10] << 8 | buff[11]);
1922 calib.Kg17.TopL = (short)((short)buff[12] << 8 | buff[13]);
1923 calib.Kg17.BottomL = (short)((short)buff[14] << 8 | buff[15]);
1924
1925 // 2nd part is scanned above
1926 }
1927 break;
1928
1929 case MOTION_PLUS:
1930 {
1931 // TODO: not known how the calibration values work
1932 changed |= MOTIONPLUS_ENABLED;
1933 bMotionPlusEnabled = true;
1934 bInitInProgress = false;
1935 // reenable reports
1936 // SetReportType(ReportType);
1937 }
1938 break;
1939 }
1940 case 0x34:
1941 {
1942 if(Internal.ExtensionType == BALANCE_BOARD)
1943 {
1944 wiimote_state::balance_board::calibration_info
1945 &calib = Internal.BalanceBoard.CalibrationInfo;
1946
1947 // 2nd part of the balance board calibration,
1948 // 34kg calibration values
1949 calib.Kg34.TopR = (short)((short)buff[0] << 8 | buff[1]);
1950 calib.Kg34.BottomR = (short)((short)buff[2] << 8 | buff[3]);
1951 calib.Kg34.TopL = (short)((short)buff[4] << 8 | buff[5]);
1952 calib.Kg34.BottomL = (short)((short)buff[6] << 8 | buff[7]);
1953
1954 changed |= BALANCE_CONNECTED;
1955 // reenable reports
1956 SetReportType(IN_BUTTONS_BALANCE_BOARD);
1957 }
1958 // else unknown what these are for
1959 }
1960 bInitInProgress = false;
1961 }
1962 break;
1963
1964 default:
1965 // _ASSERT(0); // shouldn't happen
1966 break;
1967 }
1968
1969 return changed;
1970 }
1971 // ------------------------------------------------------------------------------------
1972 void wiimote::ReadCalibration ()
1973 {
1974 TRACE(_T("Requestion wiimote calibration:"));
1975 // this appears to change the report type to 0x31
1976 ReadAddress(REGISTER_CALIBRATION, 7);
1977 }
1978 // ------------------------------------------------------------------------------------
1979 void wiimote::EnableIR (wiimote_state::ir::mode mode)
1980 {
1981 Internal.IR.Mode = mode;
1982
1983 BYTE buff [REPORT_LENGTH] = {0};
1984 buff[0] = OUT_IR;
1985 buff[1] = 0x04 | GetRumbleBit();
1986 WriteReport(buff);
1987
1988 memset(buff, 0, REPORT_LENGTH);
1989 buff[0] = OUT_IR2;
1990 buff[1] = 0x04 | GetRumbleBit();
1991 WriteReport(buff);
1992
1993 static const BYTE ir_sens1[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00,
1994 0xc0};
1995 static const BYTE ir_sens2[] = {0x40, 0x00};
1996
1997 WriteData(REGISTER_IR, 0x08);
1998 Sleep(25); // wait a little to make IR more reliable (for some)
1999 WriteData(REGISTER_IR_SENSITIVITY_1, sizeof(ir_sens1), ir_sens1);
2000 WriteData(REGISTER_IR_SENSITIVITY_2, sizeof(ir_sens2), ir_sens2);
2001 WriteData(REGISTER_IR_MODE, (BYTE)mode);
2002 }
2003 // ------------------------------------------------------------------------------------
2004 void wiimote::DisableIR ()
2005 {
2006 Internal.IR.Mode = wiimote_state::ir::OFF;
2007
2008 BYTE buff [REPORT_LENGTH] = {0};
2009 buff[0] = OUT_IR;
2010 buff[1] = GetRumbleBit();
2011 WriteReport(buff);
2012
2013 memset(buff, 0, REPORT_LENGTH);
2014 buff[0] = OUT_IR2;
2015 buff[1] = GetRumbleBit();
2016 WriteReport(buff);
2017 }
2018 // ------------------------------------------------------------------------------------
2019 unsigned __stdcall wiimote::HIDwriteThreadfunc (void* param)
2020 {
2021 _ASSERT(param);
2022 TRACE(_T("(starting HID write thread)"));
2023 wiimote &remote = *(wiimote*)param;
2024
2025 while(remote.Handle != INVALID_HANDLE_VALUE)
2026 {
2027 // try to write the oldest entry in the queue
2028 #ifdef USE_DYNAMIC_HIDQUEUE
2029 if(!remote.HIDwriteQueue.empty())
2030 #else
2031 if(!remote.HID.IsEmpty())
2032 #endif
2033 {
2034 #ifdef BEEP_DEBUG_WRITES
2035 Beep(1500,1);
2036 #endif
2037 EnterCriticalSection(&remote.HIDwriteQueueLock);
2038 #ifdef USE_DYNAMIC_HIDQUEUE
2039 BYTE *buff = remote.HIDwriteQueue.front();
2040 _ASSERT(buff);
2041 #else
2042 BYTE *buff = remote.HID.Queue[remote.HID.ReadIndex].Report;
2043 #endif
2044 LeaveCriticalSection(&remote.HIDwriteQueueLock);
2045
2046 if(!_HidD_SetOutputReport(remote.Handle, buff, REPORT_LENGTH))
2047 {
2048 DWORD err = GetLastError();
2049 if(err==ERROR_BUSY)
2050 TRACE(_T("**** HID WRITE: BUSY ****"));
2051 else if(err == ERROR_NOT_READY)
2052 TRACE(_T("**** HID WRITE: NOT READY ****"));
2053
2054 if((err != ERROR_BUSY) && // "the requested resource is in use"
2055 (err != ERROR_NOT_READY)) // "the device is not ready"
2056 {
2057 if(err == ERROR_NOT_SUPPORTED) {
2058 WARN(_T("BT Stack doesn't suport HID writes!"));
2059 goto remove_entry;
2060 }
2061 else{
2062 DEEP_TRACE(_T("HID write failed (err %u)! - "), err);
2063 // if this worked previously, the connection was probably lost
2064 if(remote.IsConnected())
2065 remote.bConnectionLost = true;
2066 }
2067 //_T("aborting write thread"), err);
2068 //return 911;
2069 }
2070 }
2071 else{
2072 remove_entry:
2073 EnterCriticalSection(&remote.HIDwriteQueueLock);
2074 #ifdef USE_DYNAMIC_HIDQUEUE
2075 remote.HIDwriteQueue.pop();
2076 delete[] buff;
2077 #else
2078 remote.HID.ReadIndex++;
2079 remote.HID.ReadIndex &= (hid::MAX_QUEUE_ENTRIES-1);
2080 #endif
2081 LeaveCriticalSection(&remote.HIDwriteQueueLock);
2082 }
2083 }
2084 Sleep(1);
2085 }
2086
2087 TRACE(_T("ending HID write thread"));
2088 return 0;
2089 }
2090 // ------------------------------------------------------------------------------------
2091 bool wiimote::WriteReport (BYTE *buff)
2092 {
2093 #ifdef BEEP_DEBUG_WRITES
2094 Beep(2000,1);
2095 #endif
2096
2097 #ifdef _DEBUG
2098 #define DEEP_TRACE_TYPE(type) case OUT_##type: DEEP_TRACE(_T("WriteReport: ")\
2099 _T(#type)); break
2100 switch(buff[0])
2101 {
2102 DEEP_TRACE_TYPE(NONE);
2103 DEEP_TRACE_TYPE(LEDs);
2104 DEEP_TRACE_TYPE(TYPE);
2105 DEEP_TRACE_TYPE(IR);
2106 DEEP_TRACE_TYPE(SPEAKER_ENABLE);
2107 DEEP_TRACE_TYPE(STATUS);
2108 DEEP_TRACE_TYPE(WRITEMEMORY);
2109 DEEP_TRACE_TYPE(READMEMORY);
2110 DEEP_TRACE_TYPE(SPEAKER_DATA);
2111 DEEP_TRACE_TYPE(SPEAKER_MUTE);
2112 DEEP_TRACE_TYPE(IR2);
2113 default:
2114 TRACE(_T("WriteReport: type [%02x][%02x]"), buff[1], buff[2]);
2115 }
2116 #endif
2117
2118 if(bUseHIDwrite)
2119 {
2120 // HidD_SetOutputReport: +: works on MS Bluetooth stacks (WriteFile doesn't).
2121 // -: is synchronous, so make it async
2122 if(!HIDwriteThread)
2123 {
2124 HIDwriteThread = (HANDLE)_beginthreadex(NULL, 0, HIDwriteThreadfunc,
2125 this, 0, NULL);
2126 _ASSERT(HIDwriteThread);
2127 if(!HIDwriteThread) {
2128 WARN(_T("couldn't create HID write thread!"));
2129 return false;
2130 }
2131 SetThreadPriority(HIDwriteThread, WORKER_THREAD_PRIORITY);
2132 }
2133
2134 // insert the write request into the thread's queue
2135 #ifdef USE_DYNAMIC_HIDQUEUE
2136 EnterCriticalSection(&HIDwriteQueueLock);
2137 BYTE *buff_copy = new BYTE[REPORT_LENGTH];
2138 #else
2139 // allocate the HID write queue once
2140 if(!HID.Queue && !HID.Allocate())
2141 return false;
2142
2143 EnterCriticalSection(&HIDwriteQueueLock);
2144 BYTE *buff_copy = HID.Queue[HID.WriteIndex].Report;
2145 #endif
2146 memcpy(buff_copy, buff, REPORT_LENGTH);
2147
2148 #ifdef USE_DYNAMIC_HIDQUEUE
2149 HIDwriteQueue.push(buff_copy);
2150 #else
2151 HID.WriteIndex++;
2152 HID.WriteIndex &= (HID.MAX_QUEUE_ENTRIES-1);
2153
2154 // check if the fixed report queue has overflown:
2155 // if this ASSERT triggers, the HID write queue (that stores reports
2156 // for asynchronous output by HIDwriteThreadfunc) has overflown.
2157 // this can happen if the connection with the wiimote has been lost
2158 // and in that case is harmless.
2159 //
2160 // if it happens during normal operation though you need to increase
2161 // hid::MAX_QUEUE_ENTRIES to the next power-of-2 (see comments)
2162 // _and_ email me the working setting so I can update the next release
2163 _ASSERT(HID.WriteIndex != HID.ReadIndex);
2164 #endif
2165 LeaveCriticalSection(&HIDwriteQueueLock);
2166 return true;
2167 }
2168
2169 // WriteFile:
2170 DWORD written;
2171 if(!WriteFile(Handle, buff, REPORT_LENGTH, &written, &Overlapped))
2172 {
2173 DWORD error = GetLastError();
2174 if(error != ERROR_IO_PENDING) {
2175 TRACE(_T("WriteFile failed, err: %u!"), error);
2176 // if it worked previously, assume we lost the connection
2177 if(IsConnected())
2178 bConnectionLost = true;
2179 #ifndef USE_DYNAMIC_HIDQUEUE
2180 HID.Deallocate();
2181 #endif
2182 return false;
2183 }
2184 }
2185 return true;
2186 }
2187 // ------------------------------------------------------------------------------------
2188 // experimental speaker support:
2189 // ------------------------------------------------------------------------------------
2190 bool wiimote::MuteSpeaker (bool on)
2191 {
2192 _ASSERT(IsConnected());
2193 if(!IsConnected())
2194 return false;
2195
2196 if(Internal.Speaker.bMuted == on)
2197 return true;
2198
2199 if(on) TRACE(_T("muting speaker." ));
2200 else TRACE(_T("unmuting speaker."));
2201
2202 BYTE buff [REPORT_LENGTH] = {0};
2203 buff[0] = OUT_SPEAKER_MUTE;
2204 buff[1] = (on? 0x04 : 0x00) | GetRumbleBit();
2205 if(!WriteReport(buff))
2206 return false;
2207 Sleep(1);
2208 Internal.Speaker.bMuted = on;
2209 return true;
2210 }
2211 // ------------------------------------------------------------------------------------
2212 bool wiimote::EnableSpeaker (bool on)
2213 {
2214 _ASSERT(IsConnected());
2215 if(!IsConnected())
2216 return false;
2217
2218 if(Internal.Speaker.bEnabled == on)
2219 return true;
2220
2221 if(on) TRACE(_T("enabling speaker.")); else TRACE(_T("disabling speaker."));
2222
2223 BYTE buff [REPORT_LENGTH] = {0};
2224 buff[0] = OUT_SPEAKER_ENABLE;
2225 buff[1] = (on? 0x04 : 0x00) | GetRumbleBit();
2226 if(!WriteReport(buff))
2227 return false;
2228
2229 if(!on) {
2230 Internal.Speaker.Freq = FREQ_NONE;
2231 Internal.Speaker.Volume = 0;
2232 MuteSpeaker(true);
2233 }
2234
2235 Internal.Speaker.bEnabled = on;
2236 return true;
2237 }
2238 // ------------------------------------------------------------------------------------
2239 #ifdef TR4 // TEMP, ignore
2240 extern int hzinc;
2241 #endif
2242 // ------------------------------------------------------------------------------------
2243 unsigned __stdcall wiimote::SampleStreamThreadfunc (void* param)
2244 {
2245 TRACE(_T("(starting sample thread)"));
2246 // sends a simple square wave sample stream
2247 wiimote &remote = *(wiimote*)param;
2248
2249 static BYTE squarewave_report[REPORT_LENGTH] =
2250 { OUT_SPEAKER_DATA, 20<<3, 0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,
2251 0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3, };
2252 static BYTE sample_report [REPORT_LENGTH] =
2253 { OUT_SPEAKER_DATA, 0 };
2254
2255 bool last_playing = false;
2256 DWORD frame = 0;
2257 DWORD frame_start = 0;
2258 unsigned total_samples = 0;
2259 unsigned sample_index = 0;
2260 wiimote_sample *current_sample = NULL;
2261
2262 // TODO: duration!!
2263 while(remote.IsConnected())
2264 {
2265 bool playing = remote.IsPlayingAudio();
2266
2267 if(!playing)
2268 Sleep(1);
2269 else{
2270 const unsigned freq_hz = FreqLookup[remote.Internal.Speaker.Freq];
2271 #ifdef TR4
2272 const float frame_ms = 1000 / ((freq_hz+hzinc) / 40.f); // 20bytes = 40 samples per write
2273 #else
2274 const float frame_ms = 1000 / (freq_hz / 40.f); // 20bytes = 40 samples per write
2275 #endif
2276
2277 // has the sample just changed?
2278 bool sample_changed = (current_sample != remote.CurrentSample);
2279 current_sample = (wiimote_sample*)remote.CurrentSample;
2280
2281 // (attempts to minimise glitches, doesn't seem to help though)
2282 //#define FIRSTFRAME_IS_SILENT // send all-zero for first frame
2283
2284 #ifdef FIRSTFRAME_IS_SILENT
2285 bool silent_frame = false;
2286 #endif
2287 if(!last_playing || sample_changed) {
2288 frame = 0;
2289 frame_start = timeGetTime();
2290 total_samples = current_sample? current_sample->length : 0;
2291 sample_index = 0;
2292 #ifdef FIRSTFRAME_IS_SILENT
2293 silent_frame = true;
2294 #endif
2295 }
2296
2297 // are we streaming a sample?
2298 if(current_sample)
2299 {
2300 if(sample_index < current_sample->length)
2301 {
2302 // (remember that samples are 4bit, ie. 2 per byte)
2303 unsigned samples_left = (current_sample->length - sample_index);
2304 unsigned report_samples = min(samples_left, (unsigned)40);
2305 // round the entries up to the nearest multiple of 2
2306 unsigned report_entries = (report_samples+1) >> 1;
2307
2308 sample_report[1] = (BYTE)((report_entries<<3) |
2309 remote.GetRumbleBit());
2310 #ifdef FIRSTFRAME_IS_SILENT
2311 if(silent_frame) {
2312 // send all-zeroes
2313 for(unsigned index=0; index<report_entries; index++)
2314 sample_report[2+index] = 0;
2315 remote.WriteReport(sample_report);
2316 }
2317 else
2318 #endif
2319 {
2320 for(unsigned index=0; index<report_entries; index++)
2321 sample_report[2+index] =
2322 current_sample->samples[(sample_index>>1)+index];
2323 remote.WriteReport(sample_report);
2324 sample_index += report_samples;
2325 }
2326 }
2327 else{
2328 // we reached the sample end
2329 remote.CurrentSample = NULL;
2330 current_sample = NULL;
2331 remote.Internal.Speaker.Freq = FREQ_NONE;
2332 remote.Internal.Speaker.Volume = 0;
2333 }
2334 }
2335 // no, a squarewave
2336 else{
2337 squarewave_report[1] = (20<<3) | remote.GetRumbleBit();
2338 remote.WriteReport(squarewave_report);
2339 #if 0
2340 // verify that we're sending at the correct rate (we are)
2341 DWORD elapsed = (timeGetTime()-frame_start);
2342 unsigned total_samples = frame * 40;
2343 float elapsed_secs = elapsed / 1000.f;
2344 float sent_persec = total_samples / elapsed_secs;
2345 #endif
2346 }
2347
2348 frame++;
2349
2350 // send the first two buffers immediately? (attempts to lessen startup
2351 // startup glitches by assuming we're filling a small sample
2352 // (or general input) buffer on the wiimote) - doesn't seem to help
2353 // if(frame > 2) {
2354 while((timeGetTime()-frame_start) < (unsigned)(frame*frame_ms))
2355 Sleep(1);
2356 // }
2357 }
2358
2359 last_playing = playing;
2360 }
2361
2362 TRACE(_T("(ending sample thread)"));
2363 return 0;
2364 }
2365 // ------------------------------------------------------------------------------------
2366 bool wiimote::Load16bitMonoSampleWAV (const TCHAR* filepath, wiimote_sample &out)
2367 {
2368 // converts unsigned 16bit mono .wav audio data to the 4bit ADPCM variant
2369 // used by the Wiimote (at least the closest match so far), and returns
2370 // the data in a BYTE array (caller must delete[] it when no longer needed):
2371 memset(&out, 0, sizeof(out));
2372
2373 TRACE(_T("Loading '%s'"), filepath);
2374
2375 FILE *file;
2376 #if (_MSC_VER >= 1400) // VC 2005+
2377 _tfopen_s(&file, filepath, _T("rb"));
2378 #else
2379 file = _tfopen(filepath, _T("rb"));
2380 #endif
2381 _ASSERT(file);
2382 if(!file) {
2383 WARN(_T("Couldn't open '%s"), filepath);
2384 return false;
2385 }
2386
2387 // parse the .wav file
2388 struct riff_chunkheader {
2389 char ckID [4];
2390 DWORD ckSize;
2391 char formType [4];
2392 };
2393 struct chunk_header {
2394 char ckID [4];
2395 DWORD ckSize;
2396 };
2397 union {
2398 WAVEFORMATEX x;
2399 WAVEFORMATEXTENSIBLE xe;
2400 } wf = {0};
2401
2402 riff_chunkheader riff_chunkheader;
2403 chunk_header chunk_header;
2404 speaker_freq freq = FREQ_NONE;
2405
2406 #define READ(data) if(fread(&data, sizeof(data), 1, file) != 1) { \
2407 TRACE(_T(".wav file corrupt")); \
2408 fclose(file); \
2409 return false; \
2410 }
2411 #define READ_SIZE(ptr,size) if(fread(ptr, size, 1, file) != 1) { \
2412 TRACE(_T(".wav file corrupt")); \
2413 fclose(file); \
2414 return false; \
2415 }
2416 // read the riff chunk header
2417 READ(riff_chunkheader);
2418
2419 // valid RIFF file?
2420 _ASSERT(!strncmp(riff_chunkheader.ckID, "RIFF", 4));
2421 if(strncmp(riff_chunkheader.ckID, "RIFF", 4))
2422 goto unsupported; // nope
2423 // valid WAV variant?
2424 _ASSERT(!strncmp(riff_chunkheader.formType, "WAVE", 4));
2425 if(strncmp(riff_chunkheader.formType, "WAVE", 4))
2426 goto unsupported; // nope
2427
2428 // find the format & data chunks
2429 while(1)
2430 {
2431 READ(chunk_header);
2432
2433 if(!strncmp(chunk_header.ckID, "fmt ", 4))
2434 {
2435 // not a valid .wav file?
2436 if(chunk_header.ckSize < 16 ||
2437 chunk_header.ckSize > sizeof(WAVEFORMATEXTENSIBLE))
2438 goto unsupported;
2439
2440 READ_SIZE((BYTE*)&wf.x, chunk_header.ckSize);
2441
2442 // now we know it's true wav file
2443 bool extensible = (wf.x.wFormatTag == WAVE_FORMAT_EXTENSIBLE);
2444 int format = extensible? wf.xe.SubFormat.Data1 :
2445 wf.x .wFormatTag;
2446 // must be uncompressed PCM (the format comparisons also work on
2447 // the 'extensible' header, even though they're named differently)
2448 if(format != WAVE_FORMAT_PCM) {
2449 TRACE(_T(".. not uncompressed PCM"));
2450 goto unsupported;
2451 }
2452
2453 // must be mono, 16bit
2454 if((wf.x.nChannels != 1) || (wf.x.wBitsPerSample != 16)) {
2455 TRACE(_T(".. %d bit, %d channel%s"), wf.x.wBitsPerSample,
2456 wf.x.nChannels,
2457 (wf.x.nChannels>1? _T("s"):_T("")));
2458 goto unsupported;
2459 }
2460
2461 // must be _near_ a supported speaker frequency range (but allow some
2462 // tolerance, especially as the speaker freq values aren't final yet):
2463 unsigned sample_freq = wf.x.nSamplesPerSec;
2464 const unsigned epsilon = 100; // for now
2465
2466 for(unsigned index=1; index<ARRAY_ENTRIES(FreqLookup); index++)
2467 {
2468 if((sample_freq+epsilon) >= FreqLookup[index] &&
2469 (sample_freq-epsilon) <= FreqLookup[index]) {
2470 freq = (speaker_freq)index;
2471 TRACE(_T(".. using speaker freq %u"), FreqLookup[index]);
2472 break;
2473 }
2474 }
2475 if(freq == FREQ_NONE) {
2476 WARN(_T("Couldn't (loosely) match .wav samplerate %u Hz to speaker"),
2477 sample_freq);
2478 goto unsupported;
2479 }
2480 }
2481 else if(!strncmp(chunk_header.ckID, "data", 4))
2482 {
2483 // make sure we got a valid fmt chunk first
2484 if(!wf.x.nBlockAlign)
2485 goto corrupt_file;
2486
2487 // grab the data
2488 unsigned total_samples = chunk_header.ckSize / wf.x.nBlockAlign;
2489 if(total_samples == 0)
2490 goto corrupt_file;
2491
2492 short *samples = new short[total_samples];
2493 size_t read = fread(samples, 2, total_samples, file);
2494 fclose(file);
2495 if(read != total_samples)
2496 {
2497 if(read == 0) {
2498 delete[] samples;
2499 goto corrupt_file;
2500 }
2501 // got a different number, but use them anyway
2502 WARN(_T("found %s .wav audio data than expected (%u/%u samples)"),
2503 ((read < total_samples)? _T("less") : _T("more")),
2504 read, total_samples);
2505
2506 total_samples = read;
2507 }
2508
2509 // and convert them
2510 bool res = Convert16bitMonoSamples(samples, true, total_samples, freq,
2511 out);
2512 delete[] samples;
2513 return res;
2514 }
2515 else{
2516 // unknown chunk, skip its data
2517 DWORD chunk_bytes = (chunk_header.ckSize + 1) & ~1L;
2518 if(fseek(file, chunk_bytes, SEEK_CUR))
2519 goto corrupt_file;
2520 }
2521 }
2522
2523 corrupt_file:
2524 WARN(_T(".wav file is corrupt"));
2525 fclose(file);
2526 return false;
2527
2528 unsupported:
2529 WARN(_T(".wav file format not supported (must be mono 16bit PCM)"));
2530 fclose(file);
2531 return false;
2532 }
2533 // ------------------------------------------------------------------------------------
2534 bool wiimote::Load16BitMonoSampleRAW (const TCHAR* filepath,
2535 bool _signed,
2536 speaker_freq freq,
2537 wiimote_sample &out)
2538 {
2539 // converts (.wav style) unsigned 16bit mono raw data to the 4bit ADPCM variant
2540 // used by the Wiimote, and returns the data in a BYTE array (caller must
2541 // delete[] it when no longer needed):
2542 memset(&out, 0, sizeof(out));
2543
2544 // get the length of the file
2545 struct _stat file_info;
2546 if(_tstat(filepath, &file_info)) {
2547 WARN(_T("couldn't get filesize for '%s'"), filepath);
2548 return false;
2549 }
2550
2551 DWORD len = file_info.st_size;
2552 _ASSERT(len);
2553 if(!len) {
2554 WARN(_T("zero-size sample file '%s'"), filepath);
2555 return false;
2556 }
2557
2558 unsigned total_samples = (len+1) / 2; // round up just in case file is corrupt
2559 // allocate a buffer to hold the samples to convert
2560 short *samples = new short[total_samples];
2561 _ASSERT(samples);
2562 if(!samples) {
2563 TRACE(_T("Couldn't open '%s"), filepath);
2564 return false;
2565 }
2566
2567 // load them
2568 FILE *file;
2569 bool res;
2570 #if (_MSC_VER >= 1400) // VC 2005+
2571 _tfopen_s(&file, filepath, _T("rb"));
2572 #else
2573 file = _tfopen(filepath, _T("rb"));
2574 #endif
2575 _ASSERT(file);
2576 if(!file) {
2577 TRACE(_T("Couldn't open '%s"), filepath);
2578 goto error;
2579 }
2580
2581 res = (fread(samples, 1, len, file) == len);
2582 fclose(file);
2583 if(!res) {
2584 WARN(_T("Couldn't load file '%s'"), filepath);
2585 goto error;
2586 }
2587
2588 // and convert them
2589 res = Convert16bitMonoSamples(samples, _signed, total_samples, freq, out);
2590 delete[] samples;
2591 return res;
2592
2593 error:
2594 delete[] samples;
2595 return false;
2596 }
2597 // ------------------------------------------------------------------------------------
2598 bool wiimote::Convert16bitMonoSamples (const short* samples,
2599 bool _signed,
2600 DWORD length,
2601 speaker_freq freq,
2602 wiimote_sample &out)
2603 {
2604 // converts 16bit mono sample data to the native 4bit format used by the Wiimote,
2605 // and returns the data in a BYTE array (caller must delete[] when no
2606 // longer needed):
2607 memset(&out, 0, sizeof(0));
2608
2609 _ASSERT(samples && length);
2610 if(!samples || !length)
2611 return false;
2612
2613 // allocate the output buffer
2614 out.samples = new BYTE[length];
2615 _ASSERT(out.samples);
2616 if(!out.samples)
2617 return false;
2618
2619 // clear it
2620 memset(out.samples, 0, length);
2621 out.length = length;
2622 out.freq = freq;
2623
2624 // ADPCM code, adapted from
2625 // http://www.wiindows.org/index.php/Talk:Wiimote#Input.2FOutput_Reports
2626 static const int index_table[16] = { -1, -1, -1, -1, 2, 4, 6, 8,
2627 -1, -1, -1, -1, 2, 4, 6, 8 };
2628 static const int diff_table [16] = { 1, 3, 5, 7, 9, 11, 13, 15,
2629 -1, -3, -5, -7, -9, -11, -13, 15 };
2630 static const int step_scale [16] = { 230, 230, 230, 230, 307, 409, 512, 614,
2631 230, 230, 230, 230, 307, 409, 512, 614 };
2632 // Encode to ADPCM, on initialization set adpcm_prev_value to 0 and adpcm_step
2633 // to 127 (these variables must be preserved across reports)
2634 int adpcm_prev_value = 0;
2635 int adpcm_step = 127;
2636
2637 for(size_t i=0; i<length; i++)
2638 {
2639 // convert to 16bit signed
2640 int value = samples[i];// (8bit) << 8);// | samples[i]; // dither it?
2641 if(!_signed)
2642 value -= 32768;
2643 // encode:
2644 int diff = value - adpcm_prev_value;
2645 BYTE encoded_val = 0;
2646 if(diff < 0) {
2647 encoded_val |= 8;
2648 diff = -diff;
2649 }
2650 diff = (diff << 2) / adpcm_step;
2651 if (diff > 7)
2652 diff = 7;
2653 encoded_val |= diff;
2654 adpcm_prev_value += ((adpcm_step * diff_table[encoded_val]) / 8);
2655 if(adpcm_prev_value > 0x7fff)
2656 adpcm_prev_value = 0x7fff;
2657 if(adpcm_prev_value < -0x8000)
2658 adpcm_prev_value = -0x8000;
2659 adpcm_step = (adpcm_step * step_scale[encoded_val]) >> 8;
2660 if(adpcm_step < 127)
2661 adpcm_step = 127;
2662 if(adpcm_step > 24567)
2663 adpcm_step = 24567;
2664 if(i & 1)
2665 out.samples[i>>1] |= encoded_val;
2666 else
2667 out.samples[i>>1] |= encoded_val << 4;
2668 }
2669
2670 return true;
2671 }
2672 // ------------------------------------------------------------------------------------
2673 bool wiimote::PlaySample (const wiimote_sample &sample, BYTE volume,
2674 speaker_freq freq_override)
2675 {
2676 _ASSERT(IsConnected());
2677 if(!IsConnected())
2678 return false;
2679
2680 speaker_freq freq = freq_override? freq_override : sample.freq;
2681
2682 TRACE(_T("playing sample."));
2683 EnableSpeaker(true);
2684 MuteSpeaker (true);
2685
2686 #if 0
2687 // combine everything into one write - faster, seems to work?
2688 BYTE bytes[9] = { 0x00, 0x00, 0x00, 10+freq, vol, 0x00, 0x00, 0x01, 0x01 };
2689 WriteData(0x04a20001, sizeof(bytes), bytes);
2690 #else
2691 // Write 0x01 to register 0x04a20009
2692 WriteData(0x04a20009, 0x01);
2693 // Write 0x08 to register 0x04a20001
2694 WriteData(0x04a20001, 0x08);
2695 // Write 7-byte configuration to registers 0x04a20001-0x04a20008
2696 BYTE bytes[7] = { 0x00, 0x00, 0x00, 10+(BYTE)freq, volume, 0x00, 0x00 };
2697 WriteData(0x04a20001, sizeof(bytes), bytes);
2698 // + Write 0x01 to register 0x04a20008
2699 WriteData(0x04a20008, 0x01);
2700 #endif
2701
2702 Internal.Speaker.Freq = freq;
2703 Internal.Speaker.Volume = volume;
2704 CurrentSample = &sample;
2705
2706 MuteSpeaker(false);
2707
2708 return StartSampleThread();
2709 }
2710 // ------------------------------------------------------------------------------------
2711 bool wiimote::StartSampleThread ()
2712 {
2713 if(SampleThread)
2714 return true;
2715
2716 SampleThread = (HANDLE)_beginthreadex(NULL, 0, SampleStreamThreadfunc,
2717 this, 0, NULL);
2718 _ASSERT(SampleThread);
2719 if(!SampleThread) {
2720 WARN(_T("couldn't create sample thread!"));
2721 MuteSpeaker (true);
2722 EnableSpeaker(false);
2723 return false;
2724 }
2725 SetThreadPriority(SampleThread, WORKER_THREAD_PRIORITY);
2726 return true;
2727 }
2728 // ------------------------------------------------------------------------------------
2729 bool wiimote::PlaySquareWave (speaker_freq freq, BYTE volume)
2730 {
2731 _ASSERT(IsConnected());
2732 if(!IsConnected())
2733 return false;
2734
2735 // if we're already playing a sample, stop it first
2736 if(IsPlayingSample())
2737 CurrentSample = NULL;
2738 // if we're already playing a square wave at this freq and volume, return
2739 else if(IsPlayingAudio() && (Internal.Speaker.Freq == freq) &&
2740 (Internal.Speaker.Volume == volume))
2741 return true;
2742
2743 TRACE(_T("playing square wave."));
2744 // stop playing samples
2745 CurrentSample = 0;
2746
2747 EnableSpeaker(true);
2748 MuteSpeaker (true);
2749
2750 #if 0
2751 // combined everything into one write - much faster, seems to work?
2752 BYTE bytes[9] = { 0x00, 0x00, 0x00, freq, volume, 0x00, 0x00, 0x01, 0x1 };
2753 WriteData(0x04a20001, sizeof(bytes), bytes);
2754 #else
2755 // write 0x01 to register 0xa20009
2756 WriteData(0x04a20009, 0x01);
2757 // write 0x08 to register 0xa20001
2758 WriteData(0x04a20001, 0x08);
2759 // write default sound mode (4bit ADPCM, we assume) 7-byte configuration
2760 // to registers 0xa20001-0xa20008
2761 BYTE bytes[7] = { 0x00, 0x00, 0x00, 10+(BYTE)freq, volume, 0x00, 0x00 };
2762 WriteData(0x04a20001, sizeof(bytes), bytes);
2763 // write 0x01 to register 0xa20008
2764 WriteData(0x04a20008, 0x01);
2765 #endif
2766
2767 Internal.Speaker.Freq = freq;
2768 Internal.Speaker.Volume = volume;
2769
2770 MuteSpeaker(false);
2771 return StartSampleThread();
2772 }
2773 // ------------------------------------------------------------------------------------
2774 void wiimote::RecordState (state_history &events_out,
2775 unsigned max_time_ms,
2776 state_change_flags change_trigger)
2777 {
2778 // user being naughty?
2779 if(Recording.bEnabled)
2780 StopRecording();
2781
2782 // clear the list
2783 if(!events_out.empty())
2784 events_out.clear();
2785
2786 // start recording
2787 Recording.StateHistory = &events_out;
2788 Recording.StartTimeMS = timeGetTime();
2789 Recording.EndTimeMS = Recording.StartTimeMS + max_time_ms;
2790 Recording.TriggerFlags = change_trigger;
2791 // as this call happens outside the read/parse thread, set the boolean
2792 // which will enable reocrding last, so that all params are in place.
2793 // TODO: * stricly speaking this only works on VC2005+ or better, as it
2794 // automatically places a memory barrier on volatile variables - earlier/
2795 // other compilers may reorder the assignments!). *
2796 Recording.bEnabled = true;
2797 }
2798 // ------------------------------------------------------------------------------------
2799 void wiimote::StopRecording ()
2800 {
2801 if(!Recording.bEnabled)
2802 return;
2803
2804 Recording.bEnabled = false;
2805 // make sure the read/parse thread has time to notice the change (else it might
2806 // still write one more state to the list)
2807 Sleep(10); // too much?
2808 }
2809 // ------------------------------------------------------------------------------------
2810 // ------------------------------------------------------------------------------------

tadas_AT_dailyda_DOT_com
ViewVC Help
Powered by ViewVC 1.1.8