1 /*---------------------------------------------------------------------------*
2 Project: KPAD library version 2
3 File: KPAD.c
4 Programmers: Keizo Ohta
5 HIRATSU Daisuke
6 Tojo Haruki
7 Tetsuya Sasaki
8
9 Copyright 2005-2008 Nintendo. All rights reserved.
10
11 These coded instructions, statements, and computer programs contain
12 proprietary information of Nintendo of America Inc. and/or Nintendo
13 Company Ltd., and are protected by Federal copyright law. They may
14 not be disclosed to third parties or copied or duplicated in any form,
15 in whole or in part, without the prior written consent of Nintendo.
16 *---------------------------------------------------------------------------*/
17
18 #include "KPADinside.h"
19
20 #include <revolution.h>
21 #include <math.h>
22 #include <string.h>
23 #include <stddef.h>
24
25 #include <revolution/revodefs.h>
26 REVOLUTION_LIB_VERSION(KPAD);
27
28
29 /*******************************************************
30 VARIABLE
31 *******************************************************/
32 //----- DPD calibration initial values
33 static Vec2 icenter_org = { 0.000f, 0.000f } ; // Center coordinate of two marks in CMOS
34 static f32 idist_org = 1.000f ; // Distance during calibration (in meters)
35 static Vec2 iaccXY_nrm_hori = { 0.000f,-1.000f } ; // Direction of XY acceleration when the controller is placed horizontally
36 static Vec2 isec_nrm_hori = { 1.000f, 0.000f } ; // Direction from left mark to right mark when the controller is placed horizontally
37 f32 kp_obj_interval = 0.200f ; // Separation between marks at each extreme (in meters)
38
39 //----- Various adjustments
40 f32 kp_acc_horizon_pw = 0.050f ; // Calculating twist from acceleration
41 f32 kp_ah_circle_radius = 0.070f ; // Static determination radius
42 f32 kp_ah_circle_pw = 0.060f ; // Static determination tracking level
43 u16 kp_ah_circle_ct = 100 ; // Static determination count
44 BOOL kp_stick_clamp_cross = FALSE ; // Perform circular clamping
45
46 //----- Numerical values regarded as errors
47 f32 kp_err_outside_frame = 0.050f ; // Width of surrounding area where center of mass coordinate is invalid (not all surrounding lights are necessarily shown)
48 f32 kp_err_dist_min ; // Automatic calculation of minimum operational distance (in meters)
49 f32 kp_err_dist_max = 3.000f ; // Maximum operational distance (in meters)
50 f32 kp_err_dist_speed = 0.040f ; // Acceptable range of change in distance (in meters)
51 f32 kp_err_first_inpr = 0.900f ; // Dot product of acceleration tilt and object tilt when selecting two points for the first time
52 f32 kp_err_next_inpr = 0.900f ; // Acceptable range of change in tilt (internal product value)
53 f32 kp_err_acc_inpr = 0.900f ; // Acceptable range for internal product with static acceleration tilt
54 f32 kp_err_up_inpr = 0.700f ; // Acceptable range for internal product with controller pointed upwards
55 f32 kp_err_near_pos = 0.100f ; // Distance from the previous point when selecting one point as a continuation
56
57 //----- For internal processing
58 //static Vec2 kobj_frame_min, kobj_frame_max ; // Range over which center of gravity coordinates are enabled
59 //static f32 kp_err_dist_speed_1 ; // Reciprocal
60 //static f32 kp_err_dist_speedM_1 ; // Negative reciprocal
61 //static f32 kp_ah_circle_radius2 ; // Square
62 static f32 kp_dist_vv1 ; // Constants
63
64 static s32 kp_fs_fstick_min = 15 ; // Nunchuk unit stick clamp settings
65 static s32 kp_fs_fstick_max = 71 ;
66 static s32 kp_cl_stick_min = 60 ; // Classic Controller unit stick clamp settings
67 static s32 kp_cl_stick_max = 308 ;
68 static s32 kp_cl_trigger_min = 30 ; // Classic Controller unit analog trigger clamp settings
69 static s32 kp_cl_trigger_max = 180 ;
70 //static s32 kp_gc_mstick_min = 15 ; // Old GC3D stick clamp setting
71 //static s32 kp_gc_mstick_max = 77 ;
72 //static s32 kp_gc_cstick_min = 15 ; // Old GCC stick clamp setting
73 //static s32 kp_gc_cstick_max = 64 ;
74 //static s32 kp_gc_trigger_min = 30 ; // Old GC analog trigger clamp setting
75 //static s32 kp_gc_trigger_max = 180 ;
76 static f32 kp_rm_acc_max = 3.4f ; // Wii Remote acceleration clamp settings
77 static f32 kp_fs_acc_max = 2.1f ; // Nunchuk acceleration clamp settings
78 static s32 kp_ex_trigger_min = 0 ; // Clamp settings for analog trigger buttons on an extension controller unit
79 static s32 kp_ex_trigger_max = 256 ;
80 static s32 kp_ex_analog_min = 0 ; // Clamp settings for analog input from an extension controller unit
81 static s32 kp_ex_analog_max = 1024 ;
82
83 static u8 kp_wbc_wait_count = 50 ; // Wait count for temperature stability on the Wii Balance Board
84 static f32 kp_wbc_ave_sample = 400 ; // Wait count for initial value stability on the Wii Balance Board
85
86 static u8 kp_initialized = 0 ; // Has KPADInit() been called?
87
88 //----- KPAD
89 KPADInsideStatus inside_kpads[ WPAD_MAX_CONTROLLERS ] ;
90
91 //----- Zero vector
92 static Vec2 Vec2_0 = { 0.0f, 0.0f } ;
93
94 //----- Used for Nunchuk acceleration correction
95 static Mtx kp_fs_rot ; // Rotation matrix
96 static f32 kp_fs_revise_deg = 24.0f ; // Correction angle (in degrees)
97
98 static void KPADiSamplingCallback ( s32 chan ) ;
99 static void KPADiConnectCallback ( s32 chan, s32 reason ) ;
100 static void KPADiControlDpdCallback ( s32 chan, s32 result ) ;
101 static void KPADiControlWbcCallback ( s32 chan, s32 result ) ;
102 static void KPADiUpdateTempWbcCallback( s32 chan, s32 result ) ;
103
104 static s32 KPADiRead( s32 chan, KPADStatus samplingBufs[], u32 length, s32 *err, BOOL keep ) ;
105
106 /*******************************************************
107 EXTERN
108 *******************************************************/
109 //----- Function to emit a warning when a callback used by the KPAD library is overwritten (this function is only used by the KPAD library)
110 extern void WPADSetCallbackByKPAD( BOOL use ) ;
111
112 // These functions are not recommended.
113 // Please use KPADGetUnifiedWpadStatus() instead.
114 // These are provided for compatibility with KPAD1. But not 100% compatible.
115
116 // These functions are not supported in SDK 3.2 or later.
117
118 #if 0
119
120 static void *get_ring_buffer_by_kpad1_style( s32 chan, void *buf, u32 size ) ;
121
122 /*******************************************************************************
123 Obtain the ring buffer of WPADStatus
124 *******************************************************************************/
125 WPADStatus *KPADGetWPADRingBuffer( s32 chan )
126 {
127 static WPADStatus status[ KPAD_RING_BUFS ] ;
128
129 return (WPADStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_CORE ) ;
130 }
131
132 WPADFSStatus *KPADGetWPADFSRingBuffer( s32 chan )
133 {
134 static WPADFSStatus status[ KPAD_RING_BUFS ] ;
135
136 return (WPADFSStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_FREESTYLE ) ;
137 }
138
139 WPADCLStatus *KPADGetWPADCLRingBuffer( s32 chan )
140 {
141 static WPADCLStatus status[ KPAD_RING_BUFS ] ;
142
143 return (WPADCLStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_CLASSIC ) ;
144 }
145
146 static void *get_ring_buffer_by_kpad1_style( s32 chan, void *buf, u32 dev )
147 {
148 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
149 BOOL enabled ;
150 s32 idx1 ;
151 s32 idx2 ;
152 u32 count ;
153 u32 size ;
154 u32 type ;
155
156 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
157
158 switch ( dev ) {
159 case WPAD_DEV_CORE :
160 size = sizeof( WPADStatus ) ;
161 break ;
162
163 case WPAD_DEV_FREESTYLE :
164 size = sizeof( WPADFSStatus ) ;
165 break ;
166
167 case WPAD_DEV_CLASSIC :
168 size = sizeof( WPADCLStatus ) ;
169 break ;
170
171 default :
172 return buf ;
173 }
174
175 enabled = OSDisableInterrupts() ;
176 idx1 = kp->bufIdx - 1 ;
177 idx2 = (s32)WPADGetLatestIndexInBuf( chan ) ;
178 for ( count = 0; count < KPAD_RING_BUFS; count++ ) {
179 if ( idx1 < 0 ) {
180 idx1 = KPAD_RING_BUFS - 1 ;
181 }
182 if ( idx2 < 0 ) {
183 idx2 = KPAD_RING_BUFS - 1 ;
184 }
185
186 switch( kp->uniRingBuf[idx1].u.core.dev ) {
187 case WPAD_DEV_CORE:
188 case WPAD_DEV_FUTURE:
189 case WPAD_DEV_NOT_SUPPORTED:
190 case WPAD_DEV_UNKNOWN:
191 type = WPAD_DEV_CORE;
192 break;
193
194 case WPAD_DEV_FREESTYLE:
195 type = WPAD_DEV_FREESTYLE;
196 break;
197
198 case WPAD_DEV_CLASSIC:
199 type = WPAD_DEV_CLASSIC;
200 break;
201
202 default:
203 // Unreachable here.
204 type = WPAD_DEV_UNKNOWN;
205 break;
206 }
207
208 if ( type == dev ) {
209 if ( WPADGetStatus() != WPAD_STATE_SETUP ) {
210 kp->uniRingBuf[idx1].u.core.err = WPAD_ERR_INVALID ;
211 }
212 memcpy((u8 *)buf + idx2 * size,
213 &kp->uniRingBuf[ idx1 ].u,
214 size) ;
215 }
216 idx1-- ;
217 idx2-- ;
218 }
219 (void)OSRestoreInterrupts( enabled ) ;
220
221 return buf ;
222 }
223 #endif
224
225 /*******************************************************************************
226 Analog data clamp setting
227 *******************************************************************************/
KPADSetFSStickClamp(s8 min,s8 max)228 void KPADSetFSStickClamp( s8 min, s8 max )
229 {
230 kp_fs_fstick_min = (s32)min ;
231 kp_fs_fstick_max = (s32)max ;
232 }
233
234
235 /*******************************************************************************
236 Configure the repeat rate of the buttons
237 *******************************************************************************/
KPADSetBtnRepeat(s32 chan,f32 delay_sec,f32 pulse_sec)238 void KPADSetBtnRepeat( s32 chan, f32 delay_sec, f32 pulse_sec )
239 {
240 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
241
242 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
243 if ( pulse_sec ) {
244 //----- Set repeat flag setting
245 kp->btn_repeat_delay = (u16)(s32)( delay_sec * 200.0f + 0.5f ) ;
246 kp->btn_repeat_pulse = (u16)(s32)( pulse_sec * 200.0f + 0.5f ) ;
247 } else {
248 //----- No repeat flag setting
249 kp->btn_repeat_delay = KPAD_BTN_NO_RPT_DELAY ;
250 kp->btn_repeat_pulse = 0 ;
251 }
252
253 //----- Reset
254 kp->btn_repeat_time = 0 ;
255 kp->btn_repeat_next = kp->btn_repeat_delay ;
256 kp->btn_cl_repeat_time = 0 ;
257 kp->btn_cl_repeat_next = kp->btn_repeat_delay ;
258 }
259
260
261 /*******************************************************************************
262 Set marker placement interval (in meters)
263 *******************************************************************************/
KPADSetObjInterval(f32 interval)264 void KPADSetObjInterval( f32 interval )
265 {
266 #pragma unused( interval )
267
268 // KPADSetObjInterval is obsolete.
269 }
270
set_obj_interval(f32 interval)271 static void set_obj_interval( f32 interval )
272 {
273 BOOL enabled = OSDisableInterrupts() ;
274
275 kp_obj_interval = interval ;
276
277 //----- DPD operational minimum distance (so that the length between marks will be half that of lens diameter)
278 kp_err_dist_min = interval / KPAD_CMOS_HFOV_TAN ;
279
280 //----- Constants for calculating distance
281 kp_dist_vv1 = interval / KPAD_CMOS_HFOV_TAN ;
282
283 (void)OSRestoreInterrupts( enabled ) ;
284
285 }
286
287
288 /*******************************************************************************
289 Parameter set
290 *******************************************************************************/
KPADSetPosParam(s32 chan,f32 play_radius,f32 sensitivity)291 void KPADSetPosParam( s32 chan, f32 play_radius, f32 sensitivity )
292 {
293 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
294 inside_kpads[ chan ].pos_play_radius = play_radius ;
295 inside_kpads[ chan ].pos_sensitivity = sensitivity ;
296 }
297
KPADSetHoriParam(s32 chan,f32 play_radius,f32 sensitivity)298 void KPADSetHoriParam( s32 chan, f32 play_radius, f32 sensitivity )
299 {
300 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) );
301 inside_kpads[ chan ].hori_play_radius = play_radius ;
302 inside_kpads[ chan ].hori_sensitivity = sensitivity ;
303 }
304
KPADSetDistParam(s32 chan,f32 play_radius,f32 sensitivity)305 void KPADSetDistParam( s32 chan, f32 play_radius, f32 sensitivity )
306 {
307 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
308 inside_kpads[ chan ].dist_play_radius = play_radius ;
309 inside_kpads[ chan ].dist_sensitivity = sensitivity ;
310 }
311
KPADSetAccParam(s32 chan,f32 play_radius,f32 sensitivity)312 void KPADSetAccParam( s32 chan, f32 play_radius, f32 sensitivity )
313 {
314 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
315 inside_kpads[ chan ].acc_play_radius = play_radius ;
316 inside_kpads[ chan ].acc_sensitivity = sensitivity ;
317 }
318
319 /*******************************************************************************
320 Get parameters
321 *******************************************************************************/
KPADGetPosParam(s32 chan,f32 * play_radius,f32 * sensitivity)322 void KPADGetPosParam( s32 chan, f32 *play_radius, f32 *sensitivity )
323 {
324 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
325 *play_radius = inside_kpads[ chan ].pos_play_radius ;
326 *sensitivity = inside_kpads[ chan ].pos_sensitivity ;
327 }
328
KPADGetHoriParam(s32 chan,f32 * play_radius,f32 * sensitivity)329 void KPADGetHoriParam( s32 chan, f32 *play_radius, f32 *sensitivity )
330 {
331 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
332 *play_radius = inside_kpads[ chan ].hori_play_radius ;
333 *sensitivity = inside_kpads[ chan ].hori_sensitivity ;
334 }
335
KPADGetDistParam(s32 chan,f32 * play_radius,f32 * sensitivity)336 void KPADGetDistParam( s32 chan, f32 *play_radius, f32 *sensitivity )
337 {
338 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
339 *play_radius = inside_kpads[ chan ].dist_play_radius ;
340 *sensitivity = inside_kpads[ chan ].dist_sensitivity ;
341 }
342
KPADGetAccParam(s32 chan,f32 * play_radius,f32 * sensitivity)343 void KPADGetAccParam( s32 chan, f32 *play_radius, f32 *sensitivity )
344 {
345 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
346 *play_radius = inside_kpads[ chan ].acc_play_radius ;
347 *sensitivity = inside_kpads[ chan ].acc_sensitivity ;
348 }
349
350
351 /*******************************************************************************
352 Seek an easy to use scale value from the calibrated center position
353 *******************************************************************************/
calc_dpd2pos_scale(KPADInsideStatus * kp)354 static void calc_dpd2pos_scale( KPADInsideStatus *kp )
355 {
356 f32 scale ;
357 f32 sx,sy ;
358
359 //----- Movable distance of the controller in the vertical and horizontal directions
360 sx = 1.0f ; // Horizontal
361 sy = (f32)KPAD_DPD_RESO_WY / (f32)KPAD_DPD_RESO_WX ; // Vertical
362
363 //----- Longest movable distance of the controller
364 scale = sqrtf( sx * sx + sy * sy ) ;// Diagonal
365
366 //----- Correct the horizontal movable distance
367 if ( kp->center_org.x < 0.0f ) {
368 sx += kp->center_org.x ;
369 } else {
370 sx -= kp->center_org.x ;
371 }
372
373 //----- Correct the vertical movable distance
374 if ( kp->center_org.y < 0.0f ) {
375 sy += kp->center_org.y ;
376 } else {
377 sy -= kp->center_org.y ;
378 }
379
380 //----- Scale that will cover the longest distance of the smaller of the range of movable distance
381 kp->dpd2pos_scale = scale / ( ( sx < sy ) ? sx : sy ) ;
382 }
383
384
385 /*******************************************************************************
386 Initialize KPAD values
387 *******************************************************************************/
reset_kpad(KPADInsideStatus * kp)388 static void reset_kpad( KPADInsideStatus *kp )
389 {
390 KPADObject *op ;
391 KPADStatus *sp = &kp->status ;
392 KPADEXStatus *ep = &kp->status.ex_status ;
393
394 kp->resetReq = FALSE ;
395
396 //----- Recalculate constants
397 kp->kobj_frame_min.x = -1.0f + kp_err_outside_frame ;
398 kp->kobj_frame_max.x = 1.0f - kp_err_outside_frame ;
399 kp->kobj_frame_min.y = -((f32)KPAD_DPD_RESO_WY / KPAD_DPD_RESO_WX) + kp_err_outside_frame ;
400 kp->kobj_frame_max.y = ((f32)KPAD_DPD_RESO_WY / KPAD_DPD_RESO_WX) - kp_err_outside_frame ;
401
402 kp->err_dist_speed_1 = 1.0f / kp_err_dist_speed ;
403 kp->err_dist_speedM_1 = -1.0f / kp_err_dist_speed ;
404 kp->ah_circle_radius2 = kp_ah_circle_radius * kp_ah_circle_radius ;
405
406 kp->err_dist_min = kp_err_dist_min ;
407 kp->dist_vv1 = kp_dist_vv1 ;
408
409 //----- Clear button information
410 sp->hold = sp->trig = sp->release = 0x00000000 ;
411 kp->btn_repeat_time = 0 ;
412 kp->btn_repeat_next = kp->btn_repeat_delay ;
413
414 //----- Clear DPD information
415 sp->dpd_valid_fg = 0 ; // Disabled
416 kp->dpd_valid2_ct = 0 ;
417
418 sp->pos = sp->vec = Vec2_0 ;
419 sp->speed = 0.0f ;
420
421 sp->horizon.x = kp->acc_horizon.x = kp->obj_horizon.x = 1.0f ;
422 sp->horizon.y = kp->acc_horizon.y = kp->obj_horizon.y = 0.0f ;
423 sp->hori_vec = Vec2_0 ;
424 sp->hori_speed = 0.0f ;
425
426 sp->acc_vertical.x = 1.0f ;
427 sp->acc_vertical.y = 0.0f ;
428
429 sp->dist = kp->dist_org ;
430 sp->dist_vec = sp->dist_speed = 0.0f ;
431
432 kp->sec_dist = sp->dist ;
433 kp->sec_length = kp->trust_sec_length = kp->dist_vv1 / kp->sec_dist ;
434 kp->sec_nrm = kp->sec_nrm_hori ;
435
436 //----- Clear acceleration information
437 sp->acc.x = sp->acc.z = 0.0f ;
438 sp->acc.y = -1.0f ;
439 sp->acc_value = 1.0f ;
440 sp->acc_speed = 0.0f ;
441 kp->hard_acc = sp->acc ;
442
443 kp->ah_circle_pos = kp->acc_horizon ;
444 kp->ah_circle_ct = kp_ah_circle_ct ;
445
446 //----- Clear individual object information
447 kp->valid_objs = 0 ;
448
449 op = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
450 do {
451 op->error_fg = -1 ; // Not applied
452 } while ( --op >= kp->kobj_sample ) ;
453
454 op = &kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ] ;
455 do {
456 op->error_fg = -1 ; // Not applied
457 } while ( --op >= kp->kobj_regular ) ;
458
459 //----- Other
460 kp->bufCount = 0 ; // Ring buffer unprocessed
461
462 //----- Clear extension controller information
463 kp->exResetReq = TRUE ;
464 }
465
466
467 /*******************************************************************************
468 Convert the DPD coordinates to projection coordinate system
469 *******************************************************************************/
KPADGetProjectionPos(Vec2 * dst,const Vec2 * src,const Rect * projRect,f32 viRatio)470 void KPADGetProjectionPos( Vec2 *dst, const Vec2 *src, const Rect *projRect, f32 viRatio )
471 {
472 f32 projection_height = projRect->bottom - projRect->top ;
473
474 // Convert the normalized values into projection coordinates.
475 (*dst).x = src->x * (projection_height / 2.0f) * 1.2f ;
476 (*dst).y = src->y * (projection_height / 2.0f) * 1.2f ;
477 // Horizontal direction pixel ratio correction
478 (*dst).x *= viRatio * 0.908 ;
479 }
480
481
482 /*******************************************************************************
483 Calibration process
484 *******************************************************************************/
KPADCalibrateDPD(s32 chan)485 s32 KPADCalibrateDPD( s32 chan )
486 {
487 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
488 KPADStatus *sp = &kp->status ;
489 KPADObject *op1, *op2 ;
490 f32 f1, vx,vy ;
491
492 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
493
494 if ( kp->valid_objs != KPAD_USE_OBJECTS ) return ( kp->valid_objs ) ;
495
496 /***********************************************************************
497 Acceleration during calibration
498 ***********************************************************************/
499 vx = kp->hard_acc.x ;
500 vy = kp->hard_acc.y ;
501 f1 = sqrtf( vx * vx + vy * vy ) ;
502 if ( f1 <= 0.5f ) return (-1) ; // Abnormal acceleration
503 kp->accXY_nrm_hori.x = vx / f1 ;
504 kp->accXY_nrm_hori.y = vy / f1 ;
505
506 /***********************************************************************
507 Object location during calibration
508 ***********************************************************************/
509 //----- Determine the mark order by location
510 op1 = kp->kobj_sample ;
511 while ( op1->error_fg != 0 ) ++op1 ;
512 op2 = op1 + 1 ;
513 while ( op2->error_fg != 0 ) ++op2 ;
514
515 if ( op1->center.x < op2->center.x ) goto LABEL_cp12 ;
516 else if ( op2->center.x < op1->center.x ) goto LABEL_cp21 ;
517 else if ( op2->center.y < op1->center.y ) goto LABEL_cp21 ;
518
519 LABEL_cp12:
520 kp->kobj_regular[ 0 ] = *op1 ;
521 kp->kobj_regular[ 1 ] = *op2 ;
522 goto LABEL_cpend ;
523
524 LABEL_cp21:
525 kp->kobj_regular[ 0 ] = *op2 ;
526 kp->kobj_regular[ 1 ] = *op1 ;
527
528 LABEL_cpend:
529
530 //kp->center_org.x = ( kp->kobj_regular[0].center.x + kp->kobj_regular[1].center.x ) * 0.5f ;
531 //kp->center_org.y = ( kp->kobj_regular[0].center.y + kp->kobj_regular[1].center.y ) * 0.5f ;
532 //kp->center_org.x = kp->center_org.y = 0.0f ;
533 calc_dpd2pos_scale( kp ) ;
534
535 //----- Section direction when the controller is in horizontal position
536 vx = kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ].center.x - kp->kobj_regular[ 0 ].center.x ;
537 vy = kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ].center.y - kp->kobj_regular[ 0 ].center.y ;
538 f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not be zero
539 kp->sec_nrm_hori.x = vx * f1 ;
540 kp->sec_nrm_hori.y = vy * f1 ;
541
542 /***********************************************************************
543 Distance during calibration
544 ***********************************************************************/
545 kp->dist_org = kp->dist_vv1 * f1 ;
546
547 /***********************************************************************
548 Other
549 ***********************************************************************/
550 sp->dpd_valid_fg = 0 ; // Invalid for now
551
552 return ( kp->valid_objs ) ;
553 }
554
555
556 /*******************************************************************************
557 Enable Wii Remote orientation correction
558 *******************************************************************************/
KPADEnableAimingMode(s32 chan)559 void KPADEnableAimingMode( s32 chan )
560 {
561 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
562 inside_kpads[ chan ].aimReq = TRUE ;
563 inside_kpads[ chan ].aimEnabled = TRUE ;
564 }
565
566
567 /*******************************************************************************
568 Disable Wii Remote orientation correction
569 *******************************************************************************/
KPADDisableAimingMode(s32 chan)570 void KPADDisableAimingMode( s32 chan )
571 {
572 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) );
573 inside_kpads[ chan ].aimReq = TRUE ;
574 inside_kpads[ chan ].aimEnabled = FALSE ;
575 }
576
577
578 /*******************************************************************************
579 Calibration process
580 *******************************************************************************/
KPADSetSensorHeight(s32 chan,f32 level)581 void KPADSetSensorHeight( s32 chan, f32 level )
582 {
583 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
584
585 kp->center_org.x = 0.0f ;
586 kp->center_org.y = -level ;
587 calc_dpd2pos_scale( kp ) ;
588 }
589
590
591 /*******************************************************************************
592 Digital button repeat processing
593 *******************************************************************************/
calc_button_repeat(KPADInsideStatus * kp,u32 dev_type,u32 count)594 static void calc_button_repeat( KPADInsideStatus *kp, u32 dev_type, u32 count )
595 {
596 KPADStatus *sp = &kp->status ;
597 KPADEXStatus *ep = &kp->status.ex_status ;
598
599 if ( sp->trig != 0 || sp->release != 0 ) {
600 //----- Reset because the button state changed
601 kp->btn_repeat_time = 0 ;
602 kp->btn_repeat_next = kp->btn_repeat_delay ;
603
604 //----- Set flags also at the beginning of button press (but only when repeat is set)
605 if ( sp->trig && kp->btn_repeat_pulse ) {
606 sp->hold |= KPAD_BUTTON_RPT ;
607 }
608 } else if ( sp->hold != 0 ) {
609 //----- Advance time because the button is pushed and its state is not changed
610 kp->btn_repeat_time += count ;
611 if ( kp->btn_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) {
612 kp->btn_repeat_time -= KPAD_BTN_NO_RPT_DELAY ;
613 }
614
615 //----- Set flag when the repeat time is reached
616 if ( kp->btn_repeat_time >= kp->btn_repeat_next ) {
617 sp->hold |= KPAD_BUTTON_RPT ;
618
619 //----- Set the next repeat time
620 kp->btn_repeat_next += kp->btn_repeat_pulse ;
621
622 //----- Loop if the time has exceeded its range here
623 if ( kp->btn_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) {
624 kp->btn_repeat_time -= KPAD_BTN_RPT_TIME_MAX ;
625 kp->btn_repeat_next -= KPAD_BTN_RPT_TIME_MAX ;
626 }
627 }
628 }
629
630 if ( dev_type == WPAD_DEV_CLASSIC
631 || dev_type == WPAD_DEV_GUITAR
632 || dev_type == WPAD_DEV_TRAIN ) {
633 if ( ep->cl.trig != 0 || ep->cl.release != 0 ) {
634 //----- Reset because the button state changed
635 kp->btn_cl_repeat_time = 0 ;
636 kp->btn_cl_repeat_next = kp->btn_repeat_delay ;
637
638 //----- Set flags also at the beginning of button press (but only when repeat is set)
639 if ( ep->cl.trig && kp->btn_repeat_pulse ) {
640 ep->cl.hold |= KPAD_BUTTON_RPT ;
641 }
642 } else if ( ep->cl.hold != 0 ) {
643 //----- Advance time because the button is pushed and its state is not changed
644 kp->btn_cl_repeat_time += count ;
645 if ( kp->btn_cl_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) {
646 kp->btn_cl_repeat_time -= KPAD_BTN_NO_RPT_DELAY ;
647 }
648
649 //----- Set flag when the repeat time is reached
650 if ( kp->btn_cl_repeat_time >= kp->btn_cl_repeat_next ) {
651 ep->cl.hold |= KPAD_BUTTON_RPT ;
652
653 //----- Set the next repeat time
654 kp->btn_cl_repeat_next += kp->btn_repeat_pulse ;
655
656 //----- Loop if the time has exceeded its range here
657 if ( kp->btn_cl_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) {
658 kp->btn_cl_repeat_time -= KPAD_BTN_RPT_TIME_MAX ;
659 kp->btn_cl_repeat_next -= KPAD_BTN_RPT_TIME_MAX ;
660 }
661 }
662 }
663 }
664 }
665
666
667 /*******************************************************************************
668 KPAD button information load process
669 *******************************************************************************/
read_kpad_button(KPADInsideStatus * kp,u32 dev_type,u32 count,u32 core,u32 fs,u32 cl)670 static void read_kpad_button( KPADInsideStatus *kp, u32 dev_type, u32 count, u32 core, u32 fs, u32 cl)
671 {
672 KPADStatus *sp = &kp->status ;
673 KPADEXStatus *ep = &kp->status.ex_status ;
674 u32 old_fg, change_fg ;
675 u32 cl_old_fg, cl_change_fg ;
676 u32 ex ;
677
678 //----- Store the previous value
679 old_fg = sp->hold & KPAD_BUTTON_MASK ;
680
681 //----- Load new value
682 sp->hold = ( core & (KPAD_BUTTON_MASK & ~(WPAD_BUTTON_Z | WPAD_BUTTON_C)) ) ;
683
684 //----- Load Nunchuk buttons
685 if ( dev_type == WPAD_DEV_FREESTYLE ) {
686 if ( kp->exResetReq ) {
687 ex = 0 ;
688 } else {
689 ex = fs ;
690 }
691 sp->hold |= ( ex & (WPAD_BUTTON_Z | WPAD_BUTTON_C) ) ;
692 }
693
694 //----- Button state processing
695 change_fg = sp->hold ^ old_fg ; // Changed button
696 sp->trig = change_fg & sp->hold ; // Pressed button
697 sp->release = change_fg & old_fg ; // Released button
698
699 //----- Load buttons for other extension controllers
700 if ( dev_type == WPAD_DEV_CLASSIC
701 || dev_type == WPAD_DEV_GUITAR
702 || dev_type == WPAD_DEV_TRAIN ) {
703 if ( kp->exResetReq ) {
704 ex = 0 ;
705 } else {
706 ex = cl ;
707 }
708
709 //----- Store the previous value
710 cl_old_fg = ep->cl.hold & KPAD_BUTTON_MASK ;
711
712 //----- Load new value
713 ep->cl.hold = ex & KPAD_BUTTON_MASK ;
714
715 //----- Button state processing
716 cl_change_fg = ep->cl.hold ^ cl_old_fg ;
717 ep->cl.trig = cl_change_fg & ep->cl.hold ;
718 ep->cl.release = cl_change_fg & cl_old_fg ;
719 }
720
721 //----- Repeat processing
722 calc_button_repeat( kp, dev_type, count ) ;
723 }
724
725
726 /*******************************************************************************
727 Acceleration tracking
728 *******************************************************************************/
calc_acc(KPADInsideStatus * kp,f32 * acc,f32 acc2)729 static void calc_acc( KPADInsideStatus *kp, f32 *acc, f32 acc2 )
730 {
731 f32 f1,f2 ;
732
733
734 //----- Difference to the target value
735 f2 = acc2 - *acc ;
736
737 if ( kp->acc_play_mode == KPAD_PLAY_MODE_LOOSE ) {
738 if ( f2 < 0.0f ) {
739 f1 = -f2 ;
740 } else {
741 f1 = f2 ;
742 }
743
744 //----- Calculation of tracking rate inside/outside the play tolerance
745 if ( f1 >= kp->acc_play_radius ) {
746 //----- Apply 100% tracking sensitivity if outside play tolerance
747 f1 = 1.0f ;
748 } else {
749 //----- If inside play tolerance, weaken tracking sensitivity as target gets closer
750 f1 /= kp->acc_play_radius ;
751 f1 *= f1 ; // Second power
752 f1 *= f1 ; // Fourth power
753 }
754 f1 *= kp->acc_sensitivity ;
755
756 //----- Tracking
757 *acc += f1 * f2 ;
758 } else {
759 if ( f2 < -kp->acc_play_radius ) {
760 *acc += ( f2 + kp->acc_play_radius ) * kp->acc_sensitivity ;
761 } else if ( f2 > kp->acc_play_radius ) {
762 *acc += ( f2 - kp->acc_play_radius ) * kp->acc_sensitivity ;
763 }
764 }
765 }
766
767
768 /*******************************************************************************
769 Calculate controller tilt from acceleration
770 *******************************************************************************/
calc_acc_horizon(KPADInsideStatus * kp)771 static void calc_acc_horizon( KPADInsideStatus *kp )
772 {
773 f32 f1, vx,vy, ax,ay ;
774
775
776 //----- XY acceleration normalization
777 f1 = sqrtf( kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ;
778 if ( f1 == 0.0f || f1 >= 2.0f ) return ;
779 ax = kp->hard_acc.x / f1 ;
780 ay = kp->hard_acc.y / f1 ;
781
782 //----- There will be more power the closer the XY acceleration length is to one (1).
783 if ( f1 > 1.0f ) {
784 f1 = 2.0f - f1 ;
785 }
786 f1 *= f1 * kp_acc_horizon_pw ;
787
788 //----- Target tilt
789 vx = kp->accXY_nrm_hori.x * ax + kp->accXY_nrm_hori.y * ay ;
790 vy = kp->accXY_nrm_hori.y * ax - kp->accXY_nrm_hori.x * ay ;
791
792 //----- Set closer
793 ax = ( vx - kp->acc_horizon.x ) * f1 + kp->acc_horizon.x ;
794 ay = ( vy - kp->acc_horizon.y ) * f1 + kp->acc_horizon.y ;
795
796 //----- Normalization
797 f1 = sqrtf( ax * ax + ay * ay ) ;
798 if ( f1 == 0.0f ) return ;
799 kp->acc_horizon.x = ax / f1 ;
800 kp->acc_horizon.y = ay / f1 ;
801
802
803 //----- Update static determination coordinate
804 kp->ah_circle_pos.x += ( kp->acc_horizon.x - kp->ah_circle_pos.x ) * kp_ah_circle_pw ;
805 kp->ah_circle_pos.y += ( kp->acc_horizon.y - kp->ah_circle_pos.y ) * kp_ah_circle_pw ;
806
807 vx = kp->acc_horizon.x - kp->ah_circle_pos.x ;
808 vy = kp->acc_horizon.y - kp->ah_circle_pos.y ;
809 if ( vx*vx + vy*vy <= kp->ah_circle_radius2 ) {
810 if ( kp->ah_circle_ct ) -- kp->ah_circle_ct ;
811 } else {
812 kp->ah_circle_ct = kp_ah_circle_ct ;
813 }
814 }
815
calc_acc_vertical(KPADInsideStatus * kp)816 static void calc_acc_vertical( KPADInsideStatus *kp )
817 {
818 KPADStatus *sp = &kp->status ;
819 f32 f1,f2, ax,ay ;
820
821
822 //----- Target tilt
823 ax = sqrtf( f2 = kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ;
824 ay = - kp->hard_acc.z ;
825 f1 = sqrtf( f2 + ay * ay ) ;
826 if ( f1 == 0.0f || f1 >= 2.0f ) return ;
827 ax /= f1 ;
828 ay /= f1 ;
829
830 //----- There will be more power the closer the acceleration length is to one (1).
831 if ( f1 > 1.0f ) {
832 f1 = 2.0f - f1 ;
833 }
834 f1 *= f1 * kp_acc_horizon_pw ;
835
836 //----- Set closer
837 ax = ( ax - sp->acc_vertical.x ) * f1 + sp->acc_vertical.x ;
838 ay = ( ay - sp->acc_vertical.y ) * f1 + sp->acc_vertical.y ;
839
840 //----- Normalization
841 f1 = sqrtf( ax * ax + ay * ay ) ;
842 if ( f1 == 0.0f ) return ;
843 sp->acc_vertical.x = ax / f1 ;
844 sp->acc_vertical.y = ay / f1 ;
845 }
846
847
848 /*******************************************************************************
849 KPAD acceleration information load process
850 *******************************************************************************/
clamp_acc(f32 acc,f32 clamp)851 static f32 clamp_acc( f32 acc, f32 clamp )
852 {
853 if ( acc < 0.0f ) {
854 if ( acc < -clamp ) return ( -clamp ) ;
855 } else {
856 if ( acc > clamp ) return ( clamp ) ;
857 }
858 return ( acc ) ;
859 }
860
read_kpad_acc(KPADInsideStatus * kp,KPADUnifiedWpadStatus * uwp)861 static void read_kpad_acc( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp )
862 {
863 KPADStatus *sp = &kp->status ;
864 Vec fsrc ;
865 Vec vec ;
866
867 switch ( uwp->fmt ) {
868 case WPAD_FMT_CORE_ACC :
869 case WPAD_FMT_CORE_ACC_DPD :
870 case WPAD_FMT_FREESTYLE_ACC :
871 case WPAD_FMT_FREESTYLE_ACC_DPD :
872 case WPAD_FMT_CLASSIC_ACC :
873 case WPAD_FMT_CLASSIC_ACC_DPD :
874 case WPAD_FMT_GUITAR :
875 // Core ACC is OK
876 break ;
877
878 default :
879 return ;
880 }
881
882 //----- Update raw values
883 kp->hard_acc.x = clamp_acc( (f32)(s32)-uwp->u.core.accX * kp->acc_scale_x, kp_rm_acc_max ) ;
884 kp->hard_acc.y = clamp_acc( (f32)(s32)-uwp->u.core.accZ * kp->acc_scale_z, kp_rm_acc_max ) ;
885 kp->hard_acc.z = clamp_acc( (f32)(s32) uwp->u.core.accY * kp->acc_scale_y, kp_rm_acc_max ) ;
886
887 //----- Temporary save
888 vec = sp->acc ;
889
890 //----- Acceleration tracking process for application use
891 calc_acc( kp, &sp->acc.x, kp->hard_acc.x ) ;
892 calc_acc( kp, &sp->acc.y, kp->hard_acc.y ) ;
893 calc_acc( kp, &sp->acc.z, kp->hard_acc.z ) ;
894 sp->acc_value = sqrtf( sp->acc.x * sp->acc.x + sp->acc.y * sp->acc.y + sp->acc.z * sp->acc.z ) ;
895
896 //----- Amount of change in acceleration, for application use
897 vec.x -= sp->acc.x ;
898 vec.y -= sp->acc.y ;
899 vec.z -= sp->acc.z ;
900 sp->acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ;
901
902 //----- Calculate controller tilt from the raw acceleration
903 calc_acc_horizon( kp ) ;
904 calc_acc_vertical( kp ) ;
905
906
907 /***********************************************************************
908 Load Nunchuk unit acceleration
909 ***********************************************************************/
910 if ( uwp->u.fs.err != WPAD_ERR_NONE ||
911 uwp->u.fs.dev != WPAD_DEV_FREESTYLE ||
912 (uwp->fmt != WPAD_FMT_FREESTYLE_ACC &&
913 uwp->fmt != WPAD_FMT_FREESTYLE_ACC_DPD) ) {
914 return ;
915 }
916
917 fsrc.x = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccX * kp->fs_acc_scale_x, kp_fs_acc_max ) ;
918 fsrc.y = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccZ * kp->fs_acc_scale_z, kp_fs_acc_max ) ;
919 fsrc.z = clamp_acc( (f32)(s32) uwp->u.fs.fsAccY * kp->fs_acc_scale_y, kp_fs_acc_max ) ;
920
921 if ( kp->fsAccRevise ) {
922 MTXMultVec( kp_fs_rot, &fsrc, &fsrc ) ;
923 }
924
925 //----- Temporary save
926 vec = sp->ex_status.fs.acc ;
927
928 //----- Acceleration tracking process for application use
929 calc_acc( kp, &sp->ex_status.fs.acc.x, fsrc.x ) ;
930 calc_acc( kp, &sp->ex_status.fs.acc.y, fsrc.y ) ;
931 calc_acc( kp, &sp->ex_status.fs.acc.z, fsrc.z ) ;
932 sp->ex_status.fs.acc_value = sqrtf( sp->ex_status.fs.acc.x * sp->ex_status.fs.acc.x + sp->ex_status.fs.acc.y * sp->ex_status.fs.acc.y + sp->ex_status.fs.acc.z * sp->ex_status.fs.acc.z ) ;
933
934 //----- Amount of change in acceleration, for application use
935 vec.x -= sp->ex_status.fs.acc.x ;
936 vec.y -= sp->ex_status.fs.acc.y ;
937 vec.z -= sp->ex_status.fs.acc.z ;
938 sp->ex_status.fs.acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ;
939 }
940
941
942 /*******************************************************************************
943 Change the WPAD object to KPAD
944 *******************************************************************************/
get_kobj(KPADInsideStatus * kp,DPDObject * wobj_p)945 static void get_kobj( KPADInsideStatus *kp, DPDObject *wobj_p )
946 {
947 const f32 dpd_scale = 2.0f / (f32)KPAD_DPD_RESO_WX ;
948 const f32 dpd_cx = (f32)( KPAD_DPD_RESO_WX - 1 ) / (f32)KPAD_DPD_RESO_WX ;
949 const f32 dpd_cy = (f32)( KPAD_DPD_RESO_WY - 1 ) / (f32)KPAD_DPD_RESO_WX ;
950
951 KPADObject *kobj_p ;
952
953 //----- Store
954 kobj_p = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
955 do {
956 if ( wobj_p->size ) {
957 //----- Valid object
958 kobj_p->center.x = (f32)(s32)wobj_p->x * dpd_scale - dpd_cx ;
959 kobj_p->center.y = (f32)(s32)wobj_p->y * dpd_scale - dpd_cy ;
960
961 kobj_p->error_fg = 0 ; // Applied
962 kobj_p->state_fg = 0 ; // Normal
963 } else {
964 //----- Invalid object
965 kobj_p->error_fg = -1 ; // Not applied
966 }
967
968 -- wobj_p ;
969 } while ( --kobj_p >= kp->kobj_sample ) ;
970 }
971
972
973 /*******************************************************************************
974 Set the surrounding objects to invalid
975 *******************************************************************************/
check_kobj_outside_frame(KPADInsideStatus * kp,KPADObject * kobj_t)976 static void check_kobj_outside_frame( KPADInsideStatus *kp, KPADObject *kobj_t )
977 {
978 KPADObject *kobj_p = &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
979
980 do {
981 if ( kobj_p->error_fg < 0 ) continue ;
982
983 if ( kobj_p->center.x <= kp->kobj_frame_min.x || kobj_p->center.x >= kp->kobj_frame_max.x ||
984 kobj_p->center.y <= kp->kobj_frame_min.y || kobj_p->center.y >= kp->kobj_frame_max.y ) {
985 kobj_p->error_fg |= 1 ;
986 }
987 } while ( --kobj_p >= kobj_t ) ;
988 }
989
990
991 /*******************************************************************************
992 Set objects at the same coordinates to invalid
993 *******************************************************************************/
check_kobj_same_position(KPADObject * kobj_t)994 static void check_kobj_same_position( KPADObject *kobj_t )
995 {
996 KPADObject *op1, *op2 ;
997
998
999 op1 = kobj_t ;
1000 do {
1001 if ( op1->error_fg != 0 ) continue ;
1002
1003 op2 = op1 + 1 ;
1004 do {
1005 if ( op2->error_fg != 0 ) continue ;
1006
1007 if ( op1->center.x == op2->center.x && op1->center.y == op2->center.y ) {
1008 op2->error_fg |= 2 ; // Set just one as error
1009 }
1010 } while ( ++op2 <= &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1011 } while ( ++op1 < &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1012 }
1013
1014
1015 /*******************************************************************************
1016 Calculate controller tilt from two points (return the distance from TV)
1017 *******************************************************************************/
calc_horizon(KPADInsideStatus * kp,Vec2 * p1,Vec2 * p2,Vec2 * hori)1018 static f32 calc_horizon( KPADInsideStatus *kp, Vec2 *p1, Vec2 *p2, Vec2 *hori )
1019 {
1020 f32 f1, vx,vy ;
1021
1022
1023 vx = p2->x - p1->x ;
1024 vy = p2->y - p1->y ;
1025 f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not be zero
1026 vx *= f1 ;
1027 vy *= f1 ;
1028
1029 hori->x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ;
1030 hori->y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ;
1031
1032 return ( kp->dist_vv1 * f1 ) ;
1033 }
1034
1035
1036 /*******************************************************************************
1037 Select two marks for the first time
1038 *******************************************************************************/
select_2obj_first(KPADInsideStatus * kp)1039 static s8 select_2obj_first( KPADInsideStatus *kp )
1040 {
1041 KPADObject *op1,*op2, *rp1,*rp2 ;
1042 Vec2 hori ;
1043 f32 f1, max = kp_err_first_inpr ;
1044
1045 op1 = kp->kobj_sample ;
1046 do {
1047 if ( op1->error_fg != 0 ) continue ;
1048
1049 op2 = op1 + 1 ;
1050 do {
1051 if ( op2->error_fg != 0 ) continue ;
1052
1053 f1 = calc_horizon( kp, &op1->center, &op2->center, &hori ) ;
1054
1055 //----- Operational distance range check
1056 if ( f1 <= kp->err_dist_min || f1 >= kp_err_dist_max ) continue ;
1057
1058 f1 = kp->acc_horizon.x * hori.x + kp->acc_horizon.y * hori.y ;
1059 if ( f1 < 0.0f ) {
1060 if ( -f1 > max ) {
1061 max = -f1 ;
1062 rp1 = op2 ;
1063 rp2 = op1 ;
1064 }
1065 } else {
1066 if ( f1 > max ) {
1067 max = f1 ;
1068 rp1 = op1 ;
1069 rp2 = op2 ;
1070 }
1071 }
1072
1073 } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1074 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1075
1076 //----- Confirmed regular mark?
1077 if ( max == kp_err_first_inpr ) return ( 0 ) ;
1078
1079 kp->kobj_regular[ 0 ] = *rp1 ;
1080 kp->kobj_regular[ 1 ] = *rp2 ;
1081
1082 return ( 2 ) ; // Two points recognition
1083 }
1084
1085
1086 /*******************************************************************************
1087 Continuing, select two marks using only the interval information
1088 *******************************************************************************/
select_2obj_continue(KPADInsideStatus * kp)1089 static s8 select_2obj_continue( KPADInsideStatus *kp )
1090 {
1091 KPADObject *op1,*op2, *rp1,*rp2 ;
1092 Vec2 nrm ;
1093 s32 rev_fg ;
1094 f32 f1,f2, vx,vy, min = 2.0f ;
1095
1096
1097 //----- Find two points closest to the previous tilt and distance
1098 op1 = kp->kobj_sample ;
1099 do {
1100 if ( op1->error_fg != 0 ) continue ;
1101
1102 op2 = op1 + 1 ;
1103 do {
1104 if ( op2->error_fg != 0 ) continue ;
1105
1106 //----- Direction calculation
1107 vx = op2->center.x - op1->center.x ;
1108 vy = op2->center.y - op1->center.y ;
1109 f1 = 1.0f / sqrtf( vx*vx + vy*vy ) ; // Should not be zero.
1110 nrm.x = vx * f1 ;
1111 nrm.y = vy * f1 ;
1112
1113 //----- Operational distance range check
1114 f1 *= kp->dist_vv1 ; // Distance
1115 if ( f1 <= kp->err_dist_min || f1 >= kp_err_dist_max ) continue ;
1116
1117 //----- Check amount of change in distance
1118 f1 -= kp->sec_dist ;
1119 if ( f1 < 0.0f ) {
1120 f1 *= kp->err_dist_speedM_1 ;
1121 } else {
1122 f1 *= kp->err_dist_speed_1 ;
1123 }
1124 if ( f1 >= 1.0f ) continue ; // Distance error rate
1125
1126 //----- Check amount of change in tilt
1127 f2 = kp->sec_nrm.x * nrm.x + kp->sec_nrm.y * nrm.y ;
1128 if ( f2 < 0.0f ) {
1129 f2 = -f2 ;
1130 rev_fg = 1 ; // Handle with orientation inverted (op2 -> op1)
1131 } else {
1132 rev_fg = 0 ; // Handle as-is (op1 -> op2)
1133 }
1134 if ( f2 <= kp_err_next_inpr ) continue ;
1135 f2 = ( 1.0f - f2 ) / ( 1.0f - kp_err_next_inpr ) ; // Tilt error rate
1136
1137 //----- Record the object with the smallest error
1138 f1 += f2 ; // Determine through the sum of distance error rate and tilt error rate
1139 if ( f1 < min ) {
1140 min = f1 ;
1141 if ( rev_fg ) {
1142 rp1 = op2 ;
1143 rp2 = op1 ;
1144 } else {
1145 rp1 = op1 ;
1146 rp2 = op2 ;
1147 }
1148 }
1149
1150 } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1151 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1152
1153 //----- Confirmed regular mark?
1154 if ( min == 2.0f ) return ( 0 ) ;
1155
1156 kp->kobj_regular[ 0 ] = *rp1 ;
1157 kp->kobj_regular[ 1 ] = *rp2 ;
1158
1159 return ( 2 ) ; // Two points recognition
1160 }
1161
1162
1163 /*******************************************************************************
1164 Select one mark for the first time
1165 *******************************************************************************/
select_1obj_first(KPADInsideStatus * kp)1166 static s8 select_1obj_first( KPADInsideStatus *kp )
1167 {
1168 KPADObject *op1 ;
1169 f32 vx,vy ;
1170 Vec2 p1,p2 ;
1171
1172
1173 //----- Determine the section direction
1174 vx = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ;
1175 vy = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ;
1176
1177 //----- Determine the section vector
1178 vx *= kp->trust_sec_length ;
1179 vy *= kp->trust_sec_length ;
1180
1181 //----- Search for a point where the expected point can be outside
1182 op1 = kp->kobj_sample ;
1183 do {
1184 if ( op1->error_fg != 0 ) continue ;
1185
1186 p1.x = op1->center.x - vx ; // Expected point is to the left
1187 p1.y = op1->center.y - vy ;
1188 p2.x = op1->center.x + vx ; // Expected point is to the right
1189 p2.y = op1->center.y + vy ;
1190
1191 if ( p1.x <= kp->kobj_frame_min.x || p1.x >= kp->kobj_frame_max.x ||
1192 p1.y <= kp->kobj_frame_min.y || p1.y >= kp->kobj_frame_max.y ) {
1193 //----- If the left expected point is outside, the right expected point needs to be inside
1194 if ( p2.x > kp->kobj_frame_min.x && p2.x < kp->kobj_frame_max.x &&
1195 p2.y > kp->kobj_frame_min.y && p2.y < kp->kobj_frame_max.y ) {
1196 //----- op1 may be the right mark
1197 kp->kobj_regular[ 1 ] = *op1 ;
1198
1199 kp->kobj_regular[ 0 ].center = p1 ;
1200 kp->kobj_regular[ 0 ].error_fg = 0 ;
1201 kp->kobj_regular[ 0 ].state_fg = -1 ;
1202
1203 return ( -1 ) ; // Recognize one unstable point
1204 }
1205 } else {
1206 //----- If the left expected point is inside, the right expected point needs to be outside.
1207 if ( p2.x <= kp->kobj_frame_min.x || p2.x >= kp->kobj_frame_max.x ||
1208 p2.y <= kp->kobj_frame_min.y || p2.y >= kp->kobj_frame_max.y ) {
1209 //----- op1 may be the left mark
1210 kp->kobj_regular[ 0 ] = *op1 ;
1211
1212 kp->kobj_regular[ 1 ].center = p2 ;
1213 kp->kobj_regular[ 1 ].error_fg = 0 ;
1214 kp->kobj_regular[ 1 ].state_fg = -1 ;
1215
1216 return ( -1 ) ; // Recognize one unstable point
1217 }
1218 }
1219
1220 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ;
1221
1222 return ( 0 ) ;
1223 }
1224
1225
1226 /*******************************************************************************
1227 Select one mark subsequently
1228 *******************************************************************************/
select_1obj_continue(KPADInsideStatus * kp)1229 static s8 select_1obj_continue( KPADInsideStatus *kp )
1230 {
1231 KPADObject *op1,*op2, *rp1,*rp2 ;
1232 f32 f1, vx,vy ;
1233 f32 min = kp_err_near_pos * kp_err_near_pos ;
1234
1235
1236 //----- Select the mark closest to the past regular mark
1237 op1 = kp->kobj_regular ;
1238 do {
1239 if ( op1->error_fg != 0 ) continue ;
1240 if ( op1->state_fg != 0 ) continue ; // No expected points
1241
1242 op2 = kp->kobj_sample ;
1243 do {
1244 if ( op2->error_fg != 0 ) continue ;
1245
1246 vx = op1->center.x - op2->center.x ;
1247 vy = op1->center.y - op2->center.y ;
1248 f1 = vx * vx + vy * vy ;
1249 if ( f1 < min ) {
1250 min = f1 ;
1251 rp1 = op1 ;
1252 rp2 = op2 ;
1253 }
1254 } while ( ++op2 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ;
1255 } while ( ++op1 < &kp->kobj_regular[ KPAD_USE_OBJECTS ] ) ;
1256
1257 //----- Confirmed regular mark?
1258 if ( min == kp_err_near_pos * kp_err_near_pos ) return ( 0 ) ;
1259
1260 *rp1 = *rp2 ;
1261
1262 //----- Calculate tilt from the acceleration
1263 kp->sec_nrm.x = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ;
1264 kp->sec_nrm.y = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ;
1265
1266 //----- Calculate expected point coordinates
1267 vx = kp->sec_length * kp->sec_nrm.x ;
1268 vy = kp->sec_length * kp->sec_nrm.y ;
1269 if ( rp1 == &kp->kobj_regular[ 0 ] ) {
1270 kp->kobj_regular[ 1 ].center.x = rp1->center.x + vx ;
1271 kp->kobj_regular[ 1 ].center.y = rp1->center.y + vy ;
1272 kp->kobj_regular[ 1 ].error_fg = 0 ;
1273 kp->kobj_regular[ 1 ].state_fg = -1 ;
1274 } else {
1275 kp->kobj_regular[ 0 ].center.x = rp1->center.x - vx ;
1276 kp->kobj_regular[ 0 ].center.y = rp1->center.y - vy ;
1277 kp->kobj_regular[ 0 ].error_fg = 0 ;
1278 kp->kobj_regular[ 0 ].state_fg = -1 ;
1279 }
1280
1281 if ( kp->status.dpd_valid_fg < 0 ) {
1282 return ( -1 ) ; // Recognize one unstable point
1283 } else {
1284 return ( 1 ) ; // One point recognition
1285 }
1286 }
1287
1288
1289 /*******************************************************************************
1290 Calculate controller tilt from object
1291 *******************************************************************************/
calc_obj_horizon(KPADInsideStatus * kp)1292 static void calc_obj_horizon( KPADInsideStatus *kp )
1293 {
1294 f32 f1, vx,vy ;
1295
1296
1297 vx = kp->kobj_regular[ 1 ].center.x - kp->kobj_regular[ 0 ].center.x ;
1298 vy = kp->kobj_regular[ 1 ].center.y - kp->kobj_regular[ 0 ].center.y ;
1299 kp->sec_length = sqrtf( vx * vx + vy * vy ) ; // Should not be zero
1300
1301 f1 = 1.0f / kp->sec_length ;
1302 kp->sec_dist = kp->dist_vv1 * f1 ;
1303
1304 kp->sec_nrm.x = ( vx *= f1 ) ;
1305 kp->sec_nrm.y = ( vy *= f1 ) ;
1306
1307 kp->obj_horizon.x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ;
1308 kp->obj_horizon.y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ;
1309 }
1310
1311
1312 /*******************************************************************************
1313 Update application variables
1314 *******************************************************************************/
calc_dpd_variable(KPADInsideStatus * kp,s8 valid_fg_next)1315 static void calc_dpd_variable( KPADInsideStatus *kp, s8 valid_fg_next )
1316 {
1317 KPADStatus *sp = &kp->status ;
1318 f32 f1,f2, dist ;
1319 Vec2 pos, vec ;
1320
1321
1322 if ( valid_fg_next == 0 ) {
1323 sp->dpd_valid_fg = 0 ;
1324 return ;
1325 }
1326
1327 /***********************************************************************
1328 Calculate the controller tilt
1329 ***********************************************************************/
1330 //----- Calculate the target value
1331 pos.x = kp->sec_nrm_hori.x * kp->sec_nrm.x + kp->sec_nrm_hori.y * kp->sec_nrm.y ;
1332 pos.y = kp->sec_nrm_hori.y * kp->sec_nrm.x - kp->sec_nrm_hori.x * kp->sec_nrm.y ;
1333
1334 //----- Consider the tracking sensitivity and play tolerance for the target value
1335 if ( sp->dpd_valid_fg == 0 ) {
1336 //----- Because this is the first pointing, initialize with the given values
1337 sp->horizon = pos ;
1338 sp->hori_vec = Vec2_0 ;
1339 sp->hori_speed = 0.0f ;
1340 } else {
1341 //----- Difference to the target value
1342 vec.x = pos.x - sp->horizon.x ;
1343 vec.y = pos.y - sp->horizon.y ;
1344 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ;
1345
1346 if ( kp->hori_play_mode == KPAD_PLAY_MODE_LOOSE ) {
1347 //----- Calculation of tracking rate inside/outside the play tolerance
1348 if ( f1 >= kp->hori_play_radius ) {
1349 //----- Apply 100% tracking sensitivity if outside play tolerance
1350 f1 = 1.0f ;
1351 } else {
1352 //----- If inside play tolerance, weaken tracking sensitivity as target gets closer
1353 f1 /= kp->hori_play_radius ;
1354 f1 *= f1 ; // Second power
1355 f1 *= f1 ; // Fourth power
1356 }
1357 f1 *= kp->hori_sensitivity ;
1358
1359 //----- Tracking
1360 vec.x = f1 * vec.x + sp->horizon.x ;
1361 vec.y = f1 * vec.y + sp->horizon.y ;
1362 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; // Normalize because this is tilt
1363 vec.x /= f1 ;
1364 vec.y /= f1 ;
1365
1366 sp->hori_vec.x = vec.x - sp->horizon.x ;
1367 sp->hori_vec.y = vec.y - sp->horizon.y ;
1368 sp->hori_speed = sqrtf( sp->hori_vec.x * sp->hori_vec.x + sp->hori_vec.y * sp->hori_vec.y ) ;
1369
1370 sp->horizon = vec ;
1371 } else {
1372 if ( f1 > kp->hori_play_radius ) {
1373 //----- Track because this is outside the play tolerance
1374 f1 = ( f1 - kp->hori_play_radius ) / f1 * kp->hori_sensitivity ;
1375 vec.x = vec.x * f1 + sp->horizon.x ;
1376 vec.y = vec.y * f1 + sp->horizon.y ;
1377 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ;
1378 vec.x /= f1 ;
1379 vec.y /= f1 ;
1380
1381 sp->hori_vec.x = vec.x - sp->horizon.x ;
1382 sp->hori_vec.y = vec.y - sp->horizon.y ;
1383 sp->hori_speed = sqrtf( sp->hori_vec.x * sp->hori_vec.x + sp->hori_vec.y * sp->hori_vec.y ) ;
1384
1385 sp->horizon = vec ;
1386 } else {
1387 //----- Do not move it because this is inside the play tolerance
1388 sp->hori_vec = Vec2_0 ;
1389 sp->hori_speed = 0.0f ;
1390 }
1391 }
1392 }
1393
1394 /***********************************************************************
1395 Calculate the distance from the TV
1396 ***********************************************************************/
1397 //----- Calculate the target value
1398 dist = kp->dist_vv1 / kp->sec_length ;
1399
1400 //----- Consider the tracking sensitivity and play tolerance for the target value
1401 if ( sp->dpd_valid_fg == 0 ) {
1402 //----- Because this is the first pointing, initialize with the given values
1403 sp->dist = dist ;
1404 sp->dist_vec = 0.0f ;
1405 sp->dist_speed = 0.0f ;
1406 } else {
1407 //----- Difference to the target value
1408 f2 = dist - sp->dist ;
1409 if ( f2 < 0.0f ) {
1410 f1 = -f2 ;
1411 } else {
1412 f1 = f2 ;
1413 }
1414
1415 if ( kp->dist_play_mode == KPAD_PLAY_MODE_LOOSE ) {
1416 //----- Calculation of tracking rate inside/outside the play tolerance
1417 if ( f1 >= kp->dist_play_radius ) {
1418 //----- Apply 100% tracking sensitivity if outside play tolerance
1419 f1 = 1.0f ;
1420 } else {
1421 //----- If inside play tolerance, weaken tracking sensitivity as target gets closer
1422 f1 /= kp->dist_play_radius ;
1423 f1 *= f1 ; // Second power
1424 f1 *= f1 ; // Fourth power
1425 }
1426 f1 *= kp->dist_sensitivity ;
1427
1428 //----- Tracking
1429 sp->dist_vec = f1 * f2 ;
1430 if ( sp->dist_vec < 0.0f ) {
1431 sp->dist_speed = -sp->dist_vec ;
1432 } else {
1433 sp->dist_speed = sp->dist_vec ;
1434 }
1435
1436 sp->dist += sp->dist_vec ;
1437 } else {
1438 if ( f1 > kp->dist_play_radius ) {
1439 //----- Track because this is outside the play tolerance
1440 f1 = ( f1 - kp->dist_play_radius ) / f1 * kp->dist_sensitivity ;
1441 sp->dist_vec = f1 * f2 ;
1442 if ( sp->dist_vec < 0.0f ) {
1443 sp->dist_speed = -sp->dist_vec ;
1444 } else {
1445 sp->dist_speed = sp->dist_vec ;
1446 }
1447
1448 sp->dist += sp->dist_vec ;
1449 } else {
1450 //----- Do not move it because this is inside the play tolerance
1451 sp->dist_vec = 0.0f ;
1452 sp->dist_speed = 0.0f ;
1453 }
1454 }
1455 }
1456
1457 /***********************************************************************
1458 Calculate the pointing location
1459 ***********************************************************************/
1460 //----- Center coordinates of two marks
1461 pos.x = ( kp->kobj_regular[ 0 ].center.x + kp->kobj_regular[ 1 ].center.x ) * 0.5f ;
1462 pos.y = ( kp->kobj_regular[ 0 ].center.y + kp->kobj_regular[ 1 ].center.y ) * 0.5f ;
1463
1464 //----- Rotate by the amount of twist
1465 f1 = kp->sec_nrm.x * kp->sec_nrm_hori.x + kp->sec_nrm.y * kp->sec_nrm_hori.y ;
1466 f2 = -kp->sec_nrm.y * kp->sec_nrm_hori.x + kp->sec_nrm.x * kp->sec_nrm_hori.y ;
1467 vec.x = f1 * pos.x - f2 * pos.y ;
1468 vec.y = f2 * pos.x + f1 * pos.y ;
1469
1470 //----- Apply scaling after correcting the center position
1471 vec.x = ( kp->center_org.x - vec.x ) * kp->dpd2pos_scale ;
1472 vec.y = ( kp->center_org.y - vec.y ) * kp->dpd2pos_scale ;
1473
1474 //----- Convert to the gravitational direction coordinate system during calibration
1475 pos.x = -kp->accXY_nrm_hori.y * vec.x + kp->accXY_nrm_hori.x * vec.y ;
1476 pos.y = -kp->accXY_nrm_hori.x * vec.x - kp->accXY_nrm_hori.y * vec.y ;
1477
1478 //----- Consider the tracking sensitivity and play tolerance for the target value
1479 if ( sp->dpd_valid_fg == 0 ) {
1480 //----- Because this is the first pointing, initialize with the given values
1481 sp->pos = pos ;
1482 sp->vec = Vec2_0 ;
1483 sp->speed = 0.0f ;
1484 } else {
1485 //----- Difference to the target value
1486 vec.x = pos.x - sp->pos.x ;
1487 vec.y = pos.y - sp->pos.y ;
1488 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ;
1489
1490 if ( kp->pos_play_mode == KPAD_PLAY_MODE_LOOSE ) {
1491 //----- Calculation of tracking rate inside/outside the play tolerance
1492 if ( f1 >= kp->pos_play_radius ) {
1493 //----- Apply 100% tracking sensitivity if outside play tolerance
1494 f1 = 1.0f ;
1495 } else {
1496 //----- If inside play tolerance, weaken tracking sensitivity as target gets closer
1497 f1 /= kp->pos_play_radius ;
1498 f1 *= f1 ; // Second power
1499 f1 *= f1 ; // Fourth power
1500 }
1501 f1 *= kp->pos_sensitivity ;
1502
1503 //----- Tracking
1504 sp->vec.x = f1 * vec.x ;
1505 sp->vec.y = f1 * vec.y ;
1506 sp->speed = sqrtf( sp->vec.x * sp->vec.x + sp->vec.y * sp->vec.y ) ;
1507
1508 sp->pos.x += sp->vec.x ;
1509 sp->pos.y += sp->vec.y ;
1510 } else {
1511 if ( f1 > kp->pos_play_radius ) {
1512 //----- Track because this is outside the play tolerance
1513 f1 = ( f1 - kp->pos_play_radius ) / f1 * kp->pos_sensitivity ;
1514 sp->vec.x = f1 * vec.x ;
1515 sp->vec.y = f1 * vec.y ;
1516 sp->speed = sqrtf( sp->vec.x * sp->vec.x + sp->vec.y * sp->vec.y ) ;
1517
1518 sp->pos.x += sp->vec.x ;
1519 sp->pos.y += sp->vec.y ;
1520 } else {
1521 //----- Do not move it because this is inside the play tolerance
1522 sp->vec = Vec2_0 ;
1523 sp->speed = 0.0f ;
1524 }
1525 }
1526 }
1527
1528 /***********************************************************************
1529 Update flags
1530 ***********************************************************************/
1531 sp->dpd_valid_fg = valid_fg_next ;
1532 }
1533
1534
1535 /*******************************************************************************
1536 KPAD DPD information load process
1537 *******************************************************************************/
read_kpad_dpd(KPADInsideStatus * kp,KPADUnifiedWpadStatus * uwp)1538 static void read_kpad_dpd( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp )
1539 {
1540 KPADStatus *sp = &kp->status ;
1541 KPADObject *op1 ;
1542 s8 valid_fg_next ;
1543
1544 /***********************************************************************
1545 Change the WPAD object to KPAD
1546 ***********************************************************************/
1547 if ( uwp->fmt == WPAD_FMT_CORE_ACC_DPD
1548 || uwp->fmt == WPAD_FMT_FREESTYLE_ACC_DPD
1549 || uwp->fmt == WPAD_FMT_CLASSIC_ACC_DPD
1550 || ( uwp->fmt == WPAD_FMT_GUITAR && kp->dpdCurrState ) ) {
1551 //----- Change the WPAD object to KPAD
1552 get_kobj( kp, &uwp->u.core.obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1553 } else {
1554 // dpd data is not prepared
1555 op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
1556 do {
1557 op1->error_fg = -1 ;
1558 } while ( --op1 >= kp->kobj_sample ) ;
1559 }
1560
1561 /***********************************************************************
1562 Select the normal object
1563 ***********************************************************************/
1564 //----- Remove untrustworthy objects
1565 check_kobj_outside_frame( kp, kp->kobj_sample ) ; // Set the surrounding objects to invalid
1566 check_kobj_same_position( kp->kobj_sample ) ; // Set the objects at the same coordinates to invalid
1567
1568 //----- Determine how many are being applied
1569 kp->valid_objs = 0 ;
1570 op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
1571 do {
1572 if ( op1->error_fg == 0 ) ++ kp->valid_objs ;
1573 } while ( --op1 >= kp->kobj_sample ) ;
1574
1575 //----- Recognition processing
1576 if ( sp->acc_vertical.x <= kp_err_up_inpr ) goto LABEL_select_NG ;
1577
1578 if ( sp->dpd_valid_fg == 2 || sp->dpd_valid_fg == -2 ) {
1579 //----- Recognized using two objects from the previous time
1580 if ( kp->valid_objs >= 2 )
1581 {
1582 valid_fg_next = select_2obj_continue( kp ) ;
1583 if ( valid_fg_next ) goto LABEL_select_OK ;
1584 }
1585 if ( kp->valid_objs >= 1 )
1586 {
1587 valid_fg_next = select_1obj_continue( kp ) ;
1588 if ( valid_fg_next ) goto LABEL_select_OK ;
1589 }
1590 } else if ( sp->dpd_valid_fg == 1 || sp->dpd_valid_fg == -1 ) {
1591 //----- Recognized using one object the previous time
1592 if ( kp->valid_objs >= 2 )
1593 {
1594 valid_fg_next = select_2obj_first( kp ) ;
1595 if ( valid_fg_next ) goto LABEL_select_OK ;
1596 }
1597 if ( kp->valid_objs >= 1 )
1598 {
1599 valid_fg_next = select_1obj_continue( kp ) ;
1600 if ( valid_fg_next ) goto LABEL_select_OK ;
1601 }
1602 } else {
1603 //----- Not recognized the previous time
1604 if ( kp->valid_objs >= 2 )
1605 {
1606 valid_fg_next = select_2obj_first( kp ) ;
1607
1608 if ( valid_fg_next ) goto LABEL_select_OK ;
1609 }
1610 if ( kp->valid_objs == 1 )
1611 {
1612 valid_fg_next = select_1obj_first( kp ) ;
1613 if ( valid_fg_next ) goto LABEL_select_OK ;
1614 }
1615 }
1616
1617 LABEL_select_NG :
1618
1619 valid_fg_next = 0 ; // Was not able to select
1620
1621 LABEL_select_OK : // Maybe was able to select
1622
1623 //----- Update section information if selected
1624 if ( valid_fg_next ) {
1625 //----- Calculate the information of two points and object tilt
1626 calc_obj_horizon( kp ) ;
1627
1628 //----- Error if obviously different from the acceleration tilt
1629 if ( kp->ah_circle_ct == 0 ) {
1630 if ( kp->obj_horizon.x * kp->acc_horizon.x + kp->obj_horizon.y * kp->acc_horizon.y <= kp_err_acc_inpr ) {
1631 valid_fg_next = 0 ; // Invalid after all
1632
1633 kp->kobj_regular[ 0 ].error_fg =
1634 kp->kobj_regular[ 1 ].error_fg = 1 ;
1635 }
1636 }
1637
1638 //----- Consecutive two point recognition count
1639 if ( sp->dpd_valid_fg == 2 && valid_fg_next == 2 ) {
1640 if ( kp->dpd_valid2_ct == 200 ) {
1641 kp->trust_sec_length = kp->sec_length ;
1642 } else {
1643 ++ kp->dpd_valid2_ct ;
1644 }
1645 } else {
1646 kp->dpd_valid2_ct = 0 ;
1647 }
1648 } else {
1649 kp->dpd_valid2_ct = 0 ;
1650 }
1651
1652 //----- Update application variables
1653 calc_dpd_variable( kp, valid_fg_next ) ;
1654 }
1655
1656
1657 /*******************************************************************************
1658 Clamp processing of analog triggers
1659 *******************************************************************************/
clamp_trigger(f32 * trigger,s32 tr,s32 min,s32 max)1660 static void clamp_trigger( f32 *trigger, s32 tr, s32 min, s32 max )
1661 {
1662 if ( tr <= min ) {
1663 *trigger = 0.0f ;
1664 } else if ( tr >= max ) {
1665 *trigger = 1.0f ;
1666 } else {
1667 *trigger = (f32)( tr - min ) / (f32)( max - min ) ;
1668 }
1669 }
1670
1671
1672 /*******************************************************************************
1673 Clamp processing of sticks
1674 *******************************************************************************/
clamp_stick_circle(Vec2 * stick,s32 sx,s32 sy,s32 min,s32 max)1675 static void clamp_stick_circle( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max )
1676 {
1677 f32 length ;
1678 f32 fx = (f32)sx ;
1679 f32 fy = (f32)sy ;
1680 f32 fmin = (f32)min ;
1681 f32 fmax = (f32)max ;
1682
1683
1684 length = sqrtf( fx * fx + fy * fy ) ;
1685
1686 if ( length <= fmin ) {
1687 stick->x = stick->y = 0.0f ;
1688
1689 } else if ( length >= fmax ) {
1690 stick->x = fx / length ;
1691 stick->y = fy / length ;
1692
1693 } else {
1694 length = ( length - fmin ) / ( fmax - fmin ) / length ;
1695 stick->x = fx * length ;
1696 stick->y = fy * length ;
1697 }
1698 }
1699
clamp_stick_cross(Vec2 * stick,s32 sx,s32 sy,s32 min,s32 max)1700 static void clamp_stick_cross( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max )
1701 {
1702 f32 length ;
1703
1704
1705 //----- Process clamp for each axis separately
1706 if ( sx < 0 ) {
1707 clamp_trigger( &stick->x, -sx, min, max ) ;
1708 stick->x = -stick->x ;
1709 } else {
1710 clamp_trigger( &stick->x, sx, min, max ) ;
1711 }
1712 if ( sy < 0 ) {
1713 clamp_trigger( &stick->y, -sy, min, max ) ;
1714 stick->y = -stick->y ;
1715 } else {
1716 clamp_trigger( &stick->y, sy, min, max ) ;
1717 }
1718
1719 //----- Set maximum length to 1
1720 length = stick->x * stick->x + stick->y * stick->y ;
1721 if ( length > 1.0f ) {
1722 length = sqrtf( length ) ;
1723 stick->x /= length ;
1724 stick->y /= length ;
1725 }
1726 }
1727
1728 /*******************************************************************************
1729 Load stick information
1730 *******************************************************************************/
read_kpad_ext(KPADInsideStatus * kp,KPADUnifiedWpadStatus * uwp)1731 static void read_kpad_ext( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp )
1732 {
1733 KPADEXStatus *ep = &kp->status.ex_status ;
1734 void (*clampStickFuncp)( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max ) ;
1735 int idx ;
1736 double tgc_weight ;
1737 s32 err ;
1738
1739 clampStickFuncp = kp_stick_clamp_cross ? clamp_stick_cross : clamp_stick_circle ;
1740
1741 if ( uwp->u.fs.dev == WPAD_DEV_FREESTYLE
1742 && (uwp->fmt == WPAD_FMT_FREESTYLE
1743 || uwp->fmt == WPAD_FMT_FREESTYLE_ACC
1744 || uwp->fmt == WPAD_FMT_FREESTYLE_ACC_DPD) ) {
1745
1746 if ( kp->exResetReq ) {
1747 kp->exResetReq = FALSE ;
1748
1749 //----- Nunchuk unit
1750 ep->fs.stick = Vec2_0 ;
1751 ep->fs.acc.x = ep->fs.acc.z = 0.0f ;
1752 ep->fs.acc.y = -1.0f ;
1753 ep->fs.acc_value = 1.0f ;
1754 ep->fs.acc_speed = 0.0f ;
1755 }
1756
1757 //----- Stick data processing
1758 clampStickFuncp( &ep->fs.stick, uwp->u.fs.fsStickX, uwp->u.fs.fsStickY, kp_fs_fstick_min, kp_fs_fstick_max ) ;
1759 } else if ( uwp->u.cl.dev == WPAD_DEV_CLASSIC
1760 && (uwp->fmt == WPAD_FMT_CLASSIC
1761 || uwp->fmt == WPAD_FMT_CLASSIC_ACC
1762 || uwp->fmt == WPAD_FMT_CLASSIC_ACC_DPD) ) {
1763
1764 if ( kp->exResetReq ) {
1765 kp->exResetReq = FALSE ;
1766
1767 //----- Classic Controller unit
1768 ep->cl.lstick = Vec2_0 ;
1769 ep->cl.rstick = Vec2_0 ;
1770 ep->cl.ltrigger = ep->cl.rtrigger = 0.0f ;
1771 ep->cl.hold = ep->cl.trig = ep->cl.release = 0x00000000 ;
1772 kp->btn_cl_repeat_time = 0 ;
1773 kp->btn_cl_repeat_next = kp->btn_repeat_delay ;
1774 }
1775
1776 //----- Stick data processing
1777 clampStickFuncp( &ep->cl.lstick, uwp->u.cl.clLStickX, uwp->u.cl.clLStickY, kp_cl_stick_min, kp_cl_stick_max ) ;
1778 clampStickFuncp( &ep->cl.rstick, uwp->u.cl.clRStickX, uwp->u.cl.clRStickY, kp_cl_stick_min, kp_cl_stick_max ) ;
1779 clamp_trigger( &ep->cl.ltrigger, uwp->u.cl.clTriggerL, kp_cl_trigger_min, kp_cl_trigger_max ) ;
1780 clamp_trigger( &ep->cl.rtrigger, uwp->u.cl.clTriggerR, kp_cl_trigger_min, kp_cl_trigger_max ) ;
1781 } else if ( (uwp->u.cl.dev == WPAD_DEV_GUITAR && uwp->fmt == WPAD_FMT_GUITAR) ) {
1782
1783 if ( kp->exResetReq ) {
1784 kp->exResetReq = FALSE ;
1785
1786 //------ Guitar or drum unit
1787 ep->cl.lstick = Vec2_0 ;
1788 ep->cl.rstick = Vec2_0 ;
1789 ep->cl.ltrigger = ep->cl.rtrigger = 0.0f ;
1790 ep->cl.hold = ep->cl.trig = ep->cl.release = 0x00000000 ;
1791 kp->btn_cl_repeat_time = 0 ;
1792 kp->btn_cl_repeat_next = kp->btn_repeat_delay ;
1793 }
1794
1795 //----- Stick data processing
1796 clampStickFuncp( &ep->cl.lstick, uwp->u.cl.clLStickX, uwp->u.cl.clLStickY, kp_cl_stick_min, kp_cl_stick_max ) ;
1797 clamp_trigger( &ep->cl.rstick.x, uwp->u.cl.clRStickX, kp_ex_analog_min, kp_ex_analog_max ) ;
1798 clamp_trigger( &ep->cl.rstick.y, uwp->u.cl.clRStickY, kp_ex_analog_min, kp_ex_analog_max ) ;
1799 clamp_trigger( &ep->cl.ltrigger, uwp->u.cl.clTriggerL, kp_ex_trigger_min, kp_ex_trigger_max ) ;
1800 clamp_trigger( &ep->cl.rtrigger, uwp->u.cl.clTriggerR, kp_ex_trigger_min, kp_ex_trigger_max ) ;
1801 } else if ( uwp->u.cl.dev == WPAD_DEV_TRAIN && uwp->fmt == WPAD_FMT_TRAIN ) {
1802 if ( kp->exResetReq ) {
1803 kp->exResetReq = FALSE ;
1804
1805 //----- Master controller unit
1806 ep->cl.lstick = Vec2_0 ;
1807 ep->cl.rstick = Vec2_0 ;
1808 ep->cl.ltrigger = ep->cl.rtrigger = 0.0f ;
1809 ep->cl.hold = ep->cl.trig = ep->cl.release = 0x00000000 ;
1810 kp->btn_cl_repeat_time = 0 ;
1811 kp->btn_cl_repeat_next = kp->btn_repeat_delay ;
1812 }
1813
1814 //----- Stick data processing
1815 ep->cl.lstick.x = ep->cl.lstick.y = 0.0f ;
1816 ep->cl.rstick.x = ep->cl.rstick.y = 0.0f ;
1817 clamp_trigger( &ep->cl.ltrigger, uwp->u.tr.brake, kp_ex_trigger_min, kp_ex_trigger_max ) ;
1818 clamp_trigger( &ep->cl.rtrigger, uwp->u.tr.mascon, kp_ex_trigger_min, kp_ex_trigger_max ) ;
1819 } else if ( uwp->u.bl.dev == WPAD_DEV_BALANCE_CHECKER && uwp->fmt == WPAD_FMT_BALANCE_CHECKER ) {
1820 if ( kp->exResetReq ) {
1821 kp->exResetReq = FALSE ;
1822
1823 KPADResetWbcZeroPoint() ;
1824 }
1825
1826 //----- Weight data processing
1827 if ( WBCGetBatteryLevel( uwp->u.bl.battery ) == 0 ) {
1828 //----- Error when the remaining battery life is below a fixed amount and the Wii Balance Board has ceased to function
1829 ep->bl.weight_err = KPAD_WBC_ERR_NO_BATTERY ;
1830 } else if ( !kp->wbcGetWeightAveDone ) {
1831 //----- Cannot be used until initial settings are complete
1832 ep->bl.weight_err = KPAD_WBC_ERR_SETUP ;
1833 } else if ( uwp->u.bl.temp == 127 || uwp->u.bl.temp == -128 ) {
1834 //----- Error when temperature information is incorrect
1835 ep->bl.weight_err = KPAD_WBC_ERR_WRONG_TEMP ;
1836 } else {
1837 ep->bl.weight_err = WBCRead( &( uwp->u.bl ), ep->bl.weight, WPAD_PRESS_UNITS ) ;
1838 if ( ep->bl.weight_err >= 0 ) {
1839 for ( idx = 0 ; idx < WPAD_PRESS_UNITS ; idx++ ) {
1840 kp->wbcWeightAve [ idx ] = ( kp->wbcWeightAve[ idx ] * kp_wbc_ave_sample + ep->bl.weight[ idx ] ) / ( kp_wbc_ave_sample + 1 ) ;
1841 ep->bl.weight_ave[ idx ] = kp->wbcWeightAve[ idx ] ;
1842 }
1843 err = WBCGetTGCWeight( ep->bl.weight_ave[ 0 ] \
1844 + ep->bl.weight_ave[ 1 ] \
1845 + ep->bl.weight_ave[ 2 ] \
1846 + ep->bl.weight_ave[ 3 ],
1847 &tgc_weight,
1848 &( uwp->u.bl ) ) ;
1849 if ( err < 0 ) {
1850 //----- Incorrect temperature information caused the error in this case
1851 ep->bl.weight_err = err ;
1852 } else if ( tgc_weight < -0.5 ) {
1853 //----- Secure a bit of margin
1854 ep->bl.weight_err = KPAD_WBC_ERR_WRONG_ZERO ;
1855 ep->bl.tgc_weight = tgc_weight ;
1856 } else {
1857 ep->bl.tgc_weight = tgc_weight ;
1858 }
1859 }
1860 }
1861 }
1862 }
1863
1864 /*******************************************************************************
1865 READ KPAD (return the number of loaded buffers)
1866 *******************************************************************************/
KPADRead(s32 chan,KPADStatus samplingBufs[],u32 length)1867 s32 KPADRead( s32 chan, KPADStatus samplingBufs[], u32 length )
1868 {
1869 return KPADiRead( chan, samplingBufs, length, NULL, FALSE ) ;
1870 }
1871
KPADReadEx(s32 chan,KPADStatus samplingBufs[],u32 length,s32 * err)1872 s32 KPADReadEx( s32 chan, KPADStatus samplingBufs[], u32 length, s32 *err )
1873 {
1874 return KPADiRead( chan, samplingBufs, length, err, TRUE ) ;
1875 }
1876
KPADiRead(s32 chan,KPADStatus samplingBufs[],u32 length,s32 * err,BOOL keep)1877 static s32 KPADiRead( s32 chan, KPADStatus samplingBufs[], u32 length, s32 *err, BOOL keep )
1878 {
1879 // This assumption is confirmed in KPADInit()
1880 // (sizeof(KPADTmpStatus) == sizeof(KPADStatus))
1881 KPADTmpStatus *tp = (KPADTmpStatus *)samplingBufs ;
1882 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
1883 KPADStatus tmp ;
1884 KPADUnifiedWpadStatus *uwp ;
1885 s32 wpad_err ;
1886 s32 kpad_err = KPAD_READ_ERR_NONE ;
1887 s32 idx ;
1888 s32 copy_ct ;
1889 s32 return_ct = 0 ;
1890 u32 bufCount ;
1891 BOOL enabled ;
1892 u32 lastCoreButton ;
1893 u32 lastFsButton ;
1894 u32 lastClButton ;
1895 u32 lastDev ;
1896 BOOL corrupted ;
1897 BOOL changed ;
1898
1899 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
1900
1901 //----- Do not process if KPADInit() has not been called
1902 if ( !kp_initialized ) {
1903 kpad_err = KPAD_READ_ERR_INIT ;
1904 goto read_finish ;
1905 }
1906 //----- Do not process until WPAD library initialization is complete
1907 if ( WPADGetStatus() != WPAD_STATE_SETUP ) {
1908 kpad_err = KPAD_READ_ERR_SETUP ;
1909 goto read_finish ;
1910 }
1911 //----- Do not process this if this was called from another thread mid-process
1912 enabled = OSDisableInterrupts() ;
1913 if ( kp->readLocked ) {
1914 (void)OSRestoreInterrupts( enabled ) ;
1915 kpad_err = KPAD_READ_ERR_LOCKED ;
1916 goto read_finish ;
1917 }
1918 kp->readLocked = TRUE ;
1919
1920 wpad_err = WPADProbe( chan, NULL ) ;
1921 if ( wpad_err == WPAD_ERR_NO_CONTROLLER ) {
1922 //----- Reset internal parameters
1923 reset_kpad( kp ) ;
1924
1925 if ( kp->dpd_ctrl_callback &&
1926 kp->dpdPreCallbackDone &&
1927 !kp->dpdPostCallbackDone ) {
1928 KPADiControlDpdCallback( chan, WPAD_ERR_NONE ) ;
1929 }
1930 //----- Remove locks
1931 kp->readLocked = FALSE ;
1932 (void)OSRestoreInterrupts( enabled ) ;
1933 kpad_err = KPAD_READ_ERR_NO_CONTROLLER ;
1934 goto read_finish ;
1935 }
1936
1937 (void)OSRestoreInterrupts( enabled ) ;
1938
1939 if ( kp->resetReq ) {
1940 // Data is invalid when a reset occurs
1941 kp->status.wpad_err = (s8)WPAD_ERR_INVALID ;
1942 reset_kpad( kp ) ;
1943 }
1944
1945 //----- Copy the first element of the buffer that was passed in
1946 tmp = samplingBufs[ 0 ] ;
1947
1948 if ( kp->bufCount > 1 &&
1949 samplingBufs &&
1950 length ) {
1951 enabled = OSDisableInterrupts() ;
1952
1953 //----- Check the number of times that data has been saved to the buffer
1954 bufCount = kp->bufCount ;
1955 if ( bufCount > length ) {
1956 bufCount = length ;
1957 }
1958 copy_ct = (s32)bufCount ;
1959
1960 //----- Flag initialization
1961 corrupted = FALSE ;
1962 changed = FALSE ;
1963 //----- Make the index match the most recent data
1964 idx = (s32) kp->bufIdx ;
1965 idx-- ;
1966 if ( idx < 0 ) {
1967 idx += kp->uniRingBufExLen + KPAD_RING_BUFS ;
1968 }
1969 //----- Get the most recent device
1970 if ( idx >= KPAD_RING_BUFS ) {
1971 uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ;
1972 } else {
1973 uwp = &kp->uniRingBuf[ idx ] ;
1974 }
1975 lastDev = uwp->u.core.dev ;
1976
1977 do {
1978 // Even if its error code is normal, the data that precedes corrupted data itself could be corrupted, so it is treated as such.
1979 //
1980 if ( corrupted ) {
1981 if ( uwp->u.core.err == WPAD_ERR_NONE ) {
1982 uwp->u.core.err = WPAD_ERR_CORRUPTED ;
1983 corrupted = FALSE ;
1984 } else if ( uwp->u.core.err == WPAD_ERR_BUSY ) {
1985 corrupted = FALSE ;
1986 }
1987 } else if ( uwp->u.core.err == WPAD_ERR_CORRUPTED ) {
1988 corrupted = TRUE ;
1989 }
1990 if ( kp->status.dev_type != uwp->u.core.dev ) {
1991 changed = TRUE ;
1992 }
1993 if ( changed ) {
1994 uwp->u.core.err = WPAD_ERR_INVALID ;
1995 }
1996 //----- Advance the index by one
1997 idx-- ;
1998 if ( idx < 0 ) {
1999 idx += kp->uniRingBufExLen + KPAD_RING_BUFS ;
2000 }
2001 if ( idx >= KPAD_RING_BUFS ) {
2002 uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ;
2003 } else {
2004 uwp = &kp->uniRingBuf[ idx ] ;
2005 }
2006 } while ( --copy_ct > 0 ) ;
2007
2008 //----- Leave the most recent data because the error check was not accurate
2009 kp->bufCount = 1 ;
2010 //----- Get the number of pieces of data in the buffer, excluding the most recent data
2011 if ( bufCount > 1 ) {
2012 copy_ct = (s32)( bufCount - 1 ) ;
2013 } else {
2014 copy_ct = 1 ;
2015 }
2016 //----- Number of pieces of data to return to the application
2017 return_ct = copy_ct ;
2018 //----- Save data to the sampling buffer, starting with the oldest
2019 tp += copy_ct ;
2020 //----- Index calculation
2021 idx = (s32)( kp->bufIdx - copy_ct - 1 ) ;
2022 if ( idx < 0 ) {
2023 idx += kp->uniRingBufExLen + KPAD_RING_BUFS ;
2024 }
2025 //----- Copy data to the buffer, starting with the oldest
2026 do {
2027 if ( idx >= KPAD_RING_BUFS ) {
2028 uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ;
2029 } else {
2030 uwp = &kp->uniRingBuf[ idx ] ;
2031 }
2032
2033 --tp ;
2034 tp->w = *uwp ;
2035 idx++ ;
2036 if ( idx >= kp->uniRingBufExLen + KPAD_RING_BUFS ) {
2037 idx = 0 ;
2038 }
2039 } while ( --copy_ct > 0 ) ;
2040
2041 (void)OSRestoreInterrupts( enabled ) ;
2042
2043 // Obtain 1G value from the controller to absorb the individual differences between controllers.
2044
2045 // Although we'd like to get the value in one try, the value cannot be retrieved immediately after connecting or attaching.
2046 // For the time being, it is obtained here each time through the loop.
2047 {
2048 WPADAcc core1G = {1, 1, 1} ;
2049 WPADAcc fs1G = {1, 1, 1} ;
2050
2051 WPADGetAccGravityUnit( chan, WPAD_DEV_CORE, &core1G ) ;
2052 if(core1G.x * core1G.y * core1G.z != 0){ // Bug fix
2053 kp->acc_scale_x = 1.0f / core1G.x ;
2054 kp->acc_scale_y = 1.0f / core1G.y ;
2055 kp->acc_scale_z = 1.0f / core1G.z ;
2056 } else {
2057 // The values listed here are obtained with WPADGetAccGravityUnit().
2058 // Accurate values should not be all that different.
2059 kp->acc_scale_x = 1.0f / 100 ;
2060 kp->acc_scale_y = 1.0f / 100 ;
2061 kp->acc_scale_z = 1.0f / 100 ;
2062 }
2063
2064 WPADGetAccGravityUnit( chan, WPAD_DEV_FREESTYLE, &fs1G ) ;
2065 if( fs1G.x * fs1G.y * fs1G.z != 0 ){ // Bug fix
2066 kp->fs_acc_scale_x = 1.0f / fs1G.x ;
2067 kp->fs_acc_scale_y = 1.0f / fs1G.y ;
2068 kp->fs_acc_scale_z = 1.0f / fs1G.z ;
2069 } else {
2070 // The values listed here are obtained with WPADGetAccGravityUnit().
2071 // Accurate values should not be all that different.
2072 kp->fs_acc_scale_x = 1.0f / 200 ;
2073 kp->fs_acc_scale_y = 1.0f / 200 ;
2074 kp->fs_acc_scale_z = 1.0f / 200 ;
2075 }
2076 }
2077
2078 //----- Process obtained data, starting with the oldest
2079 copy_ct = return_ct ;
2080 tp = (KPADTmpStatus *)samplingBufs + copy_ct ;
2081
2082 //----- Process one at a time
2083 if ( kp->btnProcMode == KPAD_BUTTON_PROC_MODE_TIGHT )
2084 {
2085 do {
2086 --tp ;
2087 uwp = &tp->w ;
2088
2089 //----- Check if the device type has changed
2090 if ( kp->status.dev_type != uwp->u.core.dev ) {
2091 kp->status.dev_type = uwp->u.core.dev ; // Update the device type
2092 kp->exResetReq = TRUE ; // Reset the extension controller information
2093 }
2094
2095 //----- Copy the error code
2096 kp->status.wpad_err = uwp->u.core.err ;
2097 //----- Copy the data format
2098 kp->status.data_format = uwp->fmt ;
2099 //----- Get the device type when data was sent
2100 lastDev = kp->status.dev_type ;
2101 //----- Error processing for button input
2102 lastCoreButton = lastFsButton = lastClButton = KPAD_BUTTON_MASK ;
2103 switch ( uwp->u.core.err ) {
2104 case WPAD_ERR_NONE :
2105 if ( lastDev == WPAD_DEV_FREESTYLE ) {
2106 lastFsButton = uwp->u.fs.button ;
2107 lastClButton = 0 ;
2108 } else if ( lastDev == WPAD_DEV_CLASSIC
2109 || lastDev == WPAD_DEV_GUITAR
2110 || lastDev == WPAD_DEV_TRAIN ) {
2111 lastFsButton = 0 ;
2112 lastClButton = uwp->u.cl.clButton ;
2113 } else {
2114 lastFsButton = lastClButton = 0 ;
2115 }
2116 lastCoreButton = (u32)( uwp->u.core.button & 0x00009F1F ) ; // Enable Wii Remote buttons only
2117 break;
2118
2119 case WPAD_ERR_CORRUPTED :
2120 lastFsButton = lastClButton = 0 ;
2121 // thru
2122 case WPAD_ERR_BUSY :
2123 lastCoreButton = (u32)( uwp->u.core.button & 0x00009F1F ) ; // Enable Wii Remote buttons only
2124 break ;
2125
2126 default :
2127 // Do nothing and keep the previous state
2128 break ;
2129 }
2130 //----- If ACK is received from the Wii Remote, use the previous button information
2131 if ( lastCoreButton == KPAD_BUTTON_MASK ) {
2132 lastCoreButton = (u32)( kp->status.hold & 0x00009F1F ) ;
2133 }
2134 if ( lastFsButton == KPAD_BUTTON_MASK ) {
2135 lastFsButton = kp->status.hold ;
2136 }
2137 if ( lastClButton == KPAD_BUTTON_MASK ) {
2138 lastClButton = kp->status.ex_status.cl.hold ;
2139 }
2140 //----- Load button information
2141 read_kpad_button( kp, lastDev, 1, lastCoreButton, lastFsButton, lastClButton ) ;
2142 //----- Miscellaneous input processing
2143 switch ( uwp->u.core.err ) {
2144 case WPAD_ERR_NONE :
2145 read_kpad_ext( kp, uwp ) ;
2146 // thru
2147 case WPAD_ERR_CORRUPTED :
2148 read_kpad_acc( kp, uwp ) ;
2149 read_kpad_dpd( kp, uwp ) ;
2150 break ;
2151
2152 default :
2153 if ( !keep ) {
2154 kp->status.dpd_valid_fg = 0 ;
2155 }
2156 break ;
2157 }
2158 tp[ 0 ].k = kp->status ;
2159
2160 } while ( --copy_ct > 0 ) ;
2161 //----- As before, the most recent button presses are applied to all data in the buffer
2162 } else {
2163 //----- Find and obtain the most recent button and device type from the buffer
2164 lastDev = WPAD_DEV_NOT_FOUND ;
2165 lastCoreButton = lastFsButton = lastClButton = KPAD_BUTTON_MASK ;
2166 do {
2167 --tp ;
2168 uwp = &tp->w ;
2169
2170 //----- Check if the device type has changed
2171 if ( kp->status.dev_type != uwp->u.core.dev ) {
2172 kp->status.dev_type = uwp->u.core.dev ;
2173 kp->exResetReq = TRUE ;
2174 }
2175 //----- Get the device type when data was sent
2176 lastDev = uwp->u.core.dev ;
2177
2178 switch ( uwp->u.core.err ) {
2179 case WPAD_ERR_NONE :
2180 if ( lastDev == WPAD_DEV_FREESTYLE ) {
2181 lastFsButton = uwp->u.fs.button ;
2182 lastClButton = 0 ;
2183 } else if ( lastDev == WPAD_DEV_CLASSIC
2184 || lastDev == WPAD_DEV_GUITAR
2185 || lastDev == WPAD_DEV_TRAIN ) {
2186 lastFsButton = 0 ;
2187 lastClButton = uwp->u.cl.clButton ;
2188 } else {
2189 lastFsButton = lastClButton = 0 ;
2190 }
2191 // thru
2192 case WPAD_ERR_CORRUPTED :
2193 case WPAD_ERR_BUSY :
2194 lastCoreButton = (u32)( uwp->u.core.button & 0x00009F1F ) ; // Enable Wii Remote buttons only
2195 break ;
2196
2197 default :
2198 break ;
2199 }
2200 } while ( --copy_ct > 0 ) ;
2201
2202 //----- If there is no valid button data, inherit the previous data
2203 if ( lastCoreButton == KPAD_BUTTON_MASK ) {
2204 lastCoreButton = (u32)( kp->status.hold & 0x00009F1F ) ;
2205 }
2206 if ( lastFsButton == KPAD_BUTTON_MASK ) {
2207 lastFsButton = kp->status.hold ;
2208 }
2209 if ( lastClButton == KPAD_BUTTON_MASK ) {
2210 lastClButton = kp->status.ex_status.cl.hold ;
2211 }
2212 //----- Process button information
2213 read_kpad_button( kp, lastDev, (u32)return_ct, lastCoreButton, lastFsButton, lastClButton ) ;
2214 //----- Start other input processing once again from the start of the buffer
2215 copy_ct = return_ct ;
2216 tp = (KPADTmpStatus *)samplingBufs + copy_ct ;
2217 do {
2218 --tp ;
2219 uwp = &tp->w ;
2220 //----- Error code copy
2221 kp->status.wpad_err = uwp->u.core.err ;
2222 //----- Copy the data format
2223 kp->status.data_format = uwp->fmt ;
2224 //----- Miscellaneous input processing
2225 switch ( uwp->u.core.err ) {
2226 case WPAD_ERR_NONE :
2227 read_kpad_ext( kp, uwp ) ;
2228 // thru
2229 case WPAD_ERR_CORRUPTED :
2230 read_kpad_acc( kp, uwp ) ;
2231 read_kpad_dpd( kp, uwp ) ;
2232 break ;
2233
2234 default :
2235 if ( !keep ) {
2236 kp->status.dpd_valid_fg = 0 ;
2237 }
2238 break ;
2239 }
2240 tp[ 0 ].k = kp->status ;
2241 } while ( --copy_ct > 0 ) ;
2242 }
2243 }
2244 //----- Remove locks
2245 kp->readLocked = FALSE ;
2246
2247 read_finish:
2248
2249 if ( return_ct == 0 ) {
2250 if ( kpad_err == KPAD_READ_ERR_NONE ) {
2251 //----- Even if there is no valid data, we'll guarantee at least the final data
2252 if ( keep ) {
2253 samplingBufs[ 0 ] = tmp ;
2254 }
2255 kpad_err = KPAD_READ_ERR_NO_DATA ;
2256 } else if ( kpad_err == KPAD_READ_ERR_NO_CONTROLLER ) {
2257 if ( keep ) {
2258 samplingBufs[ 0 ].dev_type = WPAD_DEV_NOT_FOUND ;
2259 samplingBufs[ 0 ].data_format = WPAD_FMT_CORE ;
2260 samplingBufs[ 0 ].wpad_err = WPAD_ERR_NO_CONTROLLER ;
2261 }
2262 }
2263 }
2264 if ( err ) {
2265 *err = kpad_err ;
2266 }
2267
2268 return return_ct ;
2269
2270 }
2271
2272
2273 /*******************************************************************************
2274 INIT KPAD
2275 *******************************************************************************/
KPADInit(void)2276 void KPADInit( void )
2277 {
2278 KPADInitEx( NULL, 0 );
2279 }
2280
KPADInitEx(KPADUnifiedWpadStatus uniRingBufs[],u32 length)2281 void KPADInitEx( KPADUnifiedWpadStatus uniRingBufs[], u32 length )
2282 {
2283 s32 i ;
2284 KPADInsideStatus *kp ;
2285 GXColor black = {0, 0, 0, 0} ;
2286 GXColor white = {255, 255, 255, 255} ;
2287 u32 idx ;
2288
2289 // SDK 2.0 or later is required (see SDK release note 41)
2290 // Also KPADRead() assumes: (sizeof(KPADTmpStatus) == sizeof(KPADStatus))
2291 if (offsetof( WPADFSStatus, err ) == offsetof( WPADStatus, err ) &&
2292 sizeof(KPADTmpStatus) == sizeof(KPADStatus) ) {
2293 // OK
2294 } else {
2295 OSFatal( black, white, "KPADInit error" ) ;
2296 // Never reach here.
2297 }
2298
2299 //----- Exit immediately if this is already initialized
2300 if ( kp_initialized ) {
2301 return ;
2302 }
2303
2304 //----- WPAD
2305 WPADInit() ;
2306
2307 //----- KPAD
2308 memset( &inside_kpads, 0, sizeof(inside_kpads) ) ;
2309 kp_err_dist_max = (f32)( 1.0f + (f32)WPADGetDpdSensitivity() ) ;
2310
2311 i = 0 ;
2312 do {
2313 kp = &inside_kpads[ i ] ;
2314
2315 //----- Configure the connection event callback
2316 kp->appConnectCallback = WPADSetConnectCallback( i, KPADiConnectCallback ) ;
2317 //----- Configure the sampling callback
2318 kp->appSamplingCallback = WPADSetSamplingCallback( i, KPADiSamplingCallback ) ;
2319
2320 //----- The Wii Balance Board must be initialized
2321 kp->wbcIssued = FALSE ;
2322 kp->wbcEnabled = FALSE ;
2323 kp->wbcSetup = FALSE ;
2324 KPADResetWbcZeroPoint() ;
2325
2326 //----- The DPD is enabled by default
2327 kp->dpdCurrState = FALSE ;
2328 kp->dpdNextState = TRUE ;
2329 kp->dpdCmd = WPADGetDpdFormat( i ) ;
2330
2331 //----- Check device
2332 kp->status.dev_type = WPAD_DEV_NOT_FOUND ;
2333 kp->status.data_format = WPAD_FMT_CORE ;
2334
2335 kp->dist_org = idist_org ;
2336 kp->accXY_nrm_hori = iaccXY_nrm_hori ;
2337 kp->sec_nrm_hori = isec_nrm_hori ;
2338 kp->center_org = icenter_org ;
2339 calc_dpd2pos_scale( kp ) ;
2340
2341 //----- Initialize KPAD pointing behavior
2342 kp->pos_play_radius =
2343 kp->hori_play_radius =
2344 kp->dist_play_radius =
2345 kp->acc_play_radius = 0.0f ;
2346
2347 kp->pos_sensitivity =
2348 kp->hori_sensitivity =
2349 kp->dist_sensitivity =
2350 kp->acc_sensitivity = 1.0f ;
2351
2352 kp->pos_play_mode =
2353 kp->hori_play_mode =
2354 kp->dist_play_mode =
2355 kp->acc_play_mode = KPAD_PLAY_MODE_LOOSE ;
2356
2357 //----- Initialize to no button repeat
2358 KPADSetBtnRepeat( i, 0.0f, 0.0f ) ;
2359 //----- Buttons are processed as before
2360 kp->btnProcMode = KPAD_BUTTON_PROC_MODE_LOOSE ;
2361
2362 //----- Enabling error correction for the Sensor Bar placement position
2363 KPADEnableAimingMode( i ) ;
2364
2365 //----- Nunchuk acceleration correction is disabled by default
2366 kp->fsAccRevise = 0 ;
2367
2368 if ( length > 0 && uniRingBufs ) {
2369 ASSERT( ( length % 4 ) == 0 ) ;
2370 kp->uniRingBufExLen = length / 4 ;
2371 kp->uniRingBufEx = &uniRingBufs[ i * kp->uniRingBufExLen ] ;
2372 } else {
2373 kp->uniRingBufExLen = 0 ;
2374 kp->uniRingBufEx = NULL ;
2375 }
2376
2377 for ( idx = 0 ; idx < KPAD_RING_BUFS ; idx++ ) {
2378 kp->uniRingBuf[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ;
2379 }
2380 for ( idx = 0 ; idx < kp->uniRingBufExLen ; idx++ ) {
2381 kp->uniRingBufEx[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ;
2382 }
2383
2384 } while ( ++i < WPAD_MAX_CONTROLLERS ) ;
2385
2386 //----- Cause a warning to be given when the WPAD library's callback registration function is called
2387 WPADSetCallbackByKPAD( TRUE ) ;
2388
2389 //---- Initialize values
2390 KPADReset() ;
2391
2392 //----- Rotation matrix initialization
2393 MTXRowCol( kp_fs_rot, 0, 0 ) = 1 ;
2394 MTXRowCol( kp_fs_rot, 0, 1 ) = 0 ;
2395 MTXRowCol( kp_fs_rot, 0, 2 ) = 0 ;
2396 MTXRowCol( kp_fs_rot, 0, 3 ) = 0 ;
2397 MTXRowCol( kp_fs_rot, 1, 0 ) = 0 ;
2398 MTXRowCol( kp_fs_rot, 1, 1 ) = (f32) cos( MTXDegToRad(kp_fs_revise_deg) ) ;
2399 MTXRowCol( kp_fs_rot, 1, 2 ) = (f32)-sin( MTXDegToRad(kp_fs_revise_deg) ) ;
2400 MTXRowCol( kp_fs_rot, 1, 3 ) = 0 ;
2401 MTXRowCol( kp_fs_rot, 2, 0 ) = 0 ;
2402 MTXRowCol( kp_fs_rot, 2, 1 ) = (f32) sin( MTXDegToRad(kp_fs_revise_deg) ) ;
2403 MTXRowCol( kp_fs_rot, 2, 2 ) = (f32) cos( MTXDegToRad(kp_fs_revise_deg) ) ;
2404 MTXRowCol( kp_fs_rot, 2, 3 ) = 0 ;
2405
2406 kp_initialized = 1 ;
2407
2408 OSRegisterVersion( __KPADVersion ) ;
2409 }
2410
2411 /*******************************************************************************
2412 SHUTDOWN KPAD
2413 *******************************************************************************/
KPADShutdown(void)2414 void KPADShutdown( void )
2415 {
2416 KPADInsideStatus *kp ;
2417 s32 chan ;
2418
2419 //----- Prevents a warning from being given, even if the WPAD library's callback registration function is called
2420 WPADSetCallbackByKPAD( FALSE ) ;
2421
2422 for ( chan = 0 ; chan < WPAD_MAX_CONTROLLERS ; chan++ ) {
2423 kp = &inside_kpads[ chan ] ;
2424
2425 if ( kp->appSamplingCallback ) {
2426 WPADSetSamplingCallback( chan, kp->appSamplingCallback ) ;
2427 } else {
2428 WPADSetSamplingCallback( chan, NULL ) ;
2429 }
2430 if ( kp->appConnectCallback ) {
2431 WPADSetConnectCallback( chan, kp->appConnectCallback ) ;
2432 } else {
2433 WPADSetConnectCallback( chan, NULL ) ;
2434 }
2435 }
2436
2437 //----- KPADInit() must be called to reuse KPAD
2438 kp_initialized = 0 ;
2439 }
2440
2441 /*******************************************************************************
2442 RESET KPAD
2443 *******************************************************************************/
KPADReset(void)2444 void KPADReset( void )
2445 {
2446 s32 chan ;
2447
2448 //----- Recalculate constants
2449 set_obj_interval( kp_obj_interval ) ;
2450
2451 //---- Reset all KPADs
2452 chan = WPAD_MAX_CONTROLLERS - 1 ;
2453 do {
2454 if ( WPADGetStatus() == WPAD_STATE_SETUP ) {
2455 WPADStopMotor( chan ) ;
2456 }
2457 inside_kpads[ chan ].resetReq = TRUE ;
2458 } while ( --chan >= 0 ) ;
2459 }
2460
2461 /*******************************************************************************
2462 Processing that immediately follows a connection
2463 *******************************************************************************/
KPADiConnectCallback(s32 chan,s32 reason)2464 static void KPADiConnectCallback( s32 chan, s32 reason )
2465 {
2466 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2467 u32 idx ;
2468
2469 if ( reason == WPAD_ERR_NONE ) {
2470 //----- We're going to set the sampling callback, so temporarily prevent warnings from being given
2471 WPADSetCallbackByKPAD( FALSE ) ;
2472 //----- Configure the sampling callback
2473 WPADSetSamplingCallback( chan, KPADiSamplingCallback ) ;
2474 //----- Sampling callback is set, so re-enable warnings
2475 WPADSetCallbackByKPAD( TRUE ) ;
2476 //----- Reset the DPD state
2477 kp->dpdCurrState = FALSE ;
2478 kp->dpdCmd = WPAD_DPD_OFF ;
2479 //----- Initialization is required if this is the Wii Balance Board
2480 kp->wbcIssued = FALSE ;
2481 kp->wbcEnabled = FALSE ;
2482 kp->wbcSetup = FALSE ;
2483 KPADResetWbcZeroPoint() ;
2484 } else {
2485 //----- Throw away any remaining data in the ring buffer
2486 for ( idx = 0 ; idx < KPAD_RING_BUFS ; idx++ ) {
2487 kp->uniRingBuf[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ;
2488 }
2489 for ( idx = 0 ; idx < kp->uniRingBufExLen ; idx++ ) {
2490 kp->uniRingBufEx[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ;
2491 }
2492 }
2493
2494 if ( kp->appConnectCallback ) {
2495 kp->appConnectCallback( chan, reason ) ;
2496 }
2497 }
2498
KPADSetConnectCallback(s32 chan,WPADConnectCallback callback)2499 void KPADSetConnectCallback( s32 chan, WPADConnectCallback callback )
2500 {
2501 inside_kpads[ chan ].appConnectCallback = callback ;
2502 }
2503
2504 /*******************************************************************************
2505 Wii Balance Board processing
2506 *******************************************************************************/
KPADiControlWbcCallback(s32 chan,s32 result)2507 static void KPADiControlWbcCallback( s32 chan, s32 result )
2508 {
2509 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2510
2511 kp->wbcEnabled = (u8)( ( result == WPAD_ERR_NONE ) ? TRUE : FALSE ) ;
2512 kp->wbcIssued = FALSE ;
2513 }
2514
KPADiUpdateTempWbcCallback(s32 chan,s32 result)2515 static void KPADiUpdateTempWbcCallback( s32 chan, s32 result )
2516 {
2517 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2518
2519 kp->wbcSetZeroPoint1Done = (u8)( ( result == WPAD_ERR_NONE ) ? TRUE : FALSE ) ;
2520 kp->wbcIssued = FALSE ;
2521 }
2522
KPADResetWbcZeroPoint(void)2523 void KPADResetWbcZeroPoint( void )
2524 {
2525 KPADInsideStatus *kp = &inside_kpads[ WPAD_CHAN3 ] ;
2526 int i ;
2527
2528 kp->wbcSetZeroPoint1Done = FALSE ;
2529 kp->wbcSetZeroPoint2Done = FALSE ;
2530 kp->wbcSetZeroPoint3Done = FALSE ;
2531 kp->wbcZeroPointWaitCount = 0 ;
2532 kp->wbcZeroPointSampleCount = 0 ;
2533
2534 kp->wbcGetWeightAveDone = FALSE ;
2535 kp->wbcWeightAveSampleCount = 0 ;
2536
2537 for ( i = 0 ; i < WPAD_PRESS_UNITS ; i++ ) {
2538 kp->wbcZeroPoint[ i ] = 0 ;
2539 kp->wbcWeightAve[ i ] = 0 ;
2540 }
2541 }
2542
2543 /*******************************************************************************
2544 DPD control functions
2545 *******************************************************************************/
KPADDisableDPD(const s32 chan)2546 void KPADDisableDPD( const s32 chan )
2547 {
2548 inside_kpads[ chan ].dpdNextState = FALSE ;
2549 }
2550
KPADEnableDPD(const s32 chan)2551 void KPADEnableDPD ( const s32 chan )
2552 {
2553 inside_kpads[ chan ].dpdNextState = TRUE ;
2554 }
2555
KPADSetControlDpdCallback(s32 chan,KPADControlDpdCallback callback)2556 void KPADSetControlDpdCallback( s32 chan, KPADControlDpdCallback callback )
2557 {
2558 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2559 BOOL enabled ;
2560
2561 enabled = OSDisableInterrupts() ;
2562 kp->dpd_ctrl_callback = callback ;
2563 (void)OSRestoreInterrupts( enabled ) ;
2564 }
2565
KPADiControlDpdCallback(s32 chan,s32 result)2566 static void KPADiControlDpdCallback( s32 chan, s32 result )
2567 {
2568 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2569
2570 if ( result == WPAD_ERR_NONE ) {
2571 if ( kp->dpd_ctrl_callback &&
2572 !kp->dpdPostCallbackDone ) {
2573 kp->dpdPostCallbackDone = TRUE ;
2574 kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_FINISHED ) ;
2575 kp->dpdPreCallbackDone = FALSE ;
2576 }
2577 }
2578 kp->dpdCurrState = (u8)WPADIsDpdEnabled( chan ) ;
2579 kp->dpdIssued = FALSE ;
2580 }
2581
2582 /*******************************************************************************
2583 Processing when data is obtained
2584 *******************************************************************************/
KPADiSamplingCallback(s32 chan)2585 static void KPADiSamplingCallback(s32 chan)
2586 {
2587 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2588 KPADUnifiedWpadStatus *uwp ;
2589 u32 idx ;
2590 u32 type ;
2591 u32 curDpd ;
2592 f32 aimClbr ;
2593 static struct {
2594 u8 dpd ;
2595 u8 fmt ;
2596 } table[] = {
2597 { WPAD_DPD_OFF, WPAD_FMT_CORE_ACC },
2598 { WPAD_DPD_EXP, WPAD_FMT_CORE_ACC_DPD },
2599 { WPAD_DPD_OFF, WPAD_FMT_FREESTYLE_ACC },
2600 { WPAD_DPD_STD, WPAD_FMT_FREESTYLE_ACC_DPD },
2601 { WPAD_DPD_OFF, WPAD_FMT_CLASSIC_ACC },
2602 { WPAD_DPD_STD, WPAD_FMT_CLASSIC_ACC_DPD },
2603 { WPAD_DPD_OFF, WPAD_FMT_GUITAR },
2604 { WPAD_DPD_STD, WPAD_FMT_GUITAR },
2605 { WPAD_DPD_OFF, WPAD_FMT_BALANCE_CHECKER },
2606 { WPAD_DPD_OFF, WPAD_FMT_BALANCE_CHECKER },
2607 { WPAD_DPD_OFF, WPAD_FMT_TRAIN },
2608 { WPAD_DPD_OFF, WPAD_FMT_TRAIN }
2609 } ;
2610
2611 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) );
2612
2613 if ( WPADProbe( chan, &type ) == WPAD_ERR_NO_CONTROLLER ) {
2614 goto finish ;
2615 }
2616
2617 idx = kp->bufIdx ;
2618 if ( idx >= KPAD_RING_BUFS + kp->uniRingBufExLen ) {
2619 idx = 0 ;
2620 }
2621
2622 if ( idx >= KPAD_RING_BUFS ) {
2623 uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ;
2624 } else {
2625 uwp = &kp->uniRingBuf[ idx ] ;
2626 }
2627 WPADRead( chan, &uwp->u ) ;
2628 uwp->fmt = (u8)WPADGetDataFormat( chan ) ;
2629
2630 kp->bufIdx = (u8)( idx + 1 ) ;
2631 if ( kp->bufCount < kp->uniRingBufExLen + KPAD_RING_BUFS ) {
2632 kp->bufCount++ ;
2633 }
2634
2635 if ( kp->aimReq ) {
2636 if ( kp->aimEnabled ) {
2637 if ( WPAD_SENSOR_BAR_POS_TOP == WPADGetSensorBarPosition() ) {
2638 aimClbr = 0.2f ;
2639 } else {
2640 aimClbr = -0.2f ;
2641 }
2642 } else {
2643 aimClbr = 0.0f ;
2644 }
2645 KPADSetSensorHeight( chan, aimClbr ) ;
2646
2647 kp->aimReq = FALSE ;
2648 }
2649
2650 //----- Check DPD settings
2651 switch ( type ) {
2652 case WPAD_DEV_CORE :
2653 case WPAD_DEV_FUTURE :
2654 case WPAD_DEV_NOT_SUPPORTED :
2655 case WPAD_DEV_UNKNOWN : idx = 0 ; break ;
2656 case WPAD_DEV_FREESTYLE : idx = 2 ; break ;
2657 case WPAD_DEV_CLASSIC : idx = 4 ; break ;
2658 case WPAD_DEV_GUITAR : idx = 6 ; break ;
2659 case WPAD_DEV_BALANCE_CHECKER : idx = 8 ; break ;
2660 case WPAD_DEV_TRAIN : idx = 10 ; break ;
2661 default : goto finish ; break ;
2662 }
2663
2664 if ( kp->dpdNextState ) {
2665 idx += 1 ;
2666 }
2667
2668 curDpd = (u32)( WPADIsDpdEnabled( chan ) ? kp->dpdCmd : WPAD_DPD_OFF );
2669
2670 if ( curDpd != table[idx].dpd ) {
2671 if ( kp->dpd_ctrl_callback &&
2672 !kp->dpdPreCallbackDone ) {
2673 kp->dpdPreCallbackDone = TRUE ;
2674 kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_START ) ;
2675 kp->dpdPostCallbackDone = FALSE ;
2676 }
2677 if ( !kp->dpdIssued ) {
2678 kp->dpdIssued = TRUE ;
2679 if (WPADControlDpd( chan, table[ idx ].dpd, KPADiControlDpdCallback )
2680 == WPAD_ERR_NONE ) {
2681 kp->dpdCmd = table[ idx ].dpd ;
2682 }
2683 }
2684 } else {
2685 if ( uwp->fmt != table[ idx ].fmt ) {
2686 WPADSetDataFormat( chan, table[ idx ].fmt );
2687 }
2688 }
2689
2690 //----- Initialize data required to measure weight on the Wii Balance Board
2691 if ( type == WPAD_DEV_BALANCE_CHECKER ) {
2692 if ( !kp->wbcIssued && !kp->wbcEnabled ) {
2693 //----- Start the Wii Balance Board
2694 if ( WPAD_ERR_NONE == WPADControlBLC( chan, WPAD_BLCMD_ON, KPADiControlWbcCallback ) ) {
2695 kp->wbcIssued = TRUE ;
2696 }
2697 } else if ( kp->wbcEnabled && !kp->wbcSetup ) {
2698 //----- Get the calibration values
2699 if ( !kp->wbcIssued ) {
2700 if ( WPAD_ERR_NONE == WBCSetupCalibration() ) {
2701 kp->wbcIssued = TRUE ;
2702 }
2703 } else {
2704 kp->wbcSetup = (u8)( WBCGetCalibrationStatus() ) ;
2705 kp->wbcIssued = (u8)( ( kp->wbcSetup ) ? FALSE : TRUE ) ;
2706 }
2707 } else if ( !kp->wbcIssued && kp->wbcSetup && !kp->wbcSetZeroPoint1Done ) {
2708 //----- Update the temperature
2709 if ( WPAD_ERR_NONE == WPADControlBLC( chan, WPAD_BLCMD_UPDATE_TEMP, KPADiUpdateTempWbcCallback ) ) {
2710 kp->wbcIssued = TRUE ;
2711 }
2712 } else if ( kp->wbcSetZeroPoint1Done && !kp->wbcSetZeroPoint2Done ) {
2713 //----- Wait until the temperature has stabilized
2714 if ( uwp->u.bl.temp == 127 || uwp->u.bl.temp == -128 ) {
2715 KPADResetWbcZeroPoint() ;
2716 } else if ( ++kp->wbcZeroPointWaitCount > kp_wbc_wait_count ) {
2717 kp->wbcSetZeroPoint2Done = TRUE ;
2718 }
2719 } else if ( kp->wbcSetZeroPoint2Done && !kp->wbcSetZeroPoint3Done ) {
2720 //----- Get the zero-point correction values
2721 if ( uwp->u.bl.err == WPAD_ERR_NONE ) {
2722 kp->wbcZeroPointSampleCount++ ;
2723 kp->wbcZeroPoint[ 0 ] = ( kp->wbcZeroPoint[ 0 ] * ( kp->wbcZeroPointSampleCount - 1 ) + uwp->u.bl.press[ 0 ] ) / kp->wbcZeroPointSampleCount ;
2724 kp->wbcZeroPoint[ 1 ] = ( kp->wbcZeroPoint[ 1 ] * ( kp->wbcZeroPointSampleCount - 1 ) + uwp->u.bl.press[ 1 ] ) / kp->wbcZeroPointSampleCount ;
2725 kp->wbcZeroPoint[ 2 ] = ( kp->wbcZeroPoint[ 2 ] * ( kp->wbcZeroPointSampleCount - 1 ) + uwp->u.bl.press[ 2 ] ) / kp->wbcZeroPointSampleCount ;
2726 kp->wbcZeroPoint[ 3 ] = ( kp->wbcZeroPoint[ 3 ] * ( kp->wbcZeroPointSampleCount - 1 ) + uwp->u.bl.press[ 3 ] ) / kp->wbcZeroPointSampleCount ;
2727
2728 if ( kp->wbcZeroPointSampleCount > kp_wbc_ave_sample ) {
2729 kp->wbcSetZeroPoint3Done = TRUE ;
2730 WBCSetZEROPoint( kp->wbcZeroPoint, WPAD_PRESS_UNITS ) ;
2731 }
2732 }
2733 } else if ( kp->wbcSetZeroPoint3Done && !kp->wbcGetWeightAveDone ) {
2734 //----- Take the average weight for each sensor to get the initial weight values
2735 if ( uwp->u.bl.err == WPAD_ERR_NONE ) {
2736 double weight[ WPAD_PRESS_UNITS ] ;
2737 if ( WBCRead( &( uwp->u.bl ), weight, WPAD_PRESS_UNITS ) >= 0 ) {
2738 kp->wbcWeightAveSampleCount++ ;
2739 kp->wbcWeightAve[ 0 ] = ( kp->wbcWeightAve[ 0 ] * ( kp->wbcWeightAveSampleCount - 1 ) + weight[ 0 ] ) / kp->wbcWeightAveSampleCount ;
2740 kp->wbcWeightAve[ 1 ] = ( kp->wbcWeightAve[ 1 ] * ( kp->wbcWeightAveSampleCount - 1 ) + weight[ 1 ] ) / kp->wbcWeightAveSampleCount ;
2741 kp->wbcWeightAve[ 2 ] = ( kp->wbcWeightAve[ 2 ] * ( kp->wbcWeightAveSampleCount - 1 ) + weight[ 2 ] ) / kp->wbcWeightAveSampleCount ;
2742 kp->wbcWeightAve[ 3 ] = ( kp->wbcWeightAve[ 3 ] * ( kp->wbcWeightAveSampleCount - 1 ) + weight[ 3 ] ) / kp->wbcWeightAveSampleCount ;
2743
2744 if ( kp->wbcWeightAveSampleCount > kp_wbc_ave_sample ) {
2745 kp->wbcGetWeightAveDone = TRUE ;
2746 }
2747 }
2748 }
2749 }
2750 }
2751
2752 finish :
2753 if ( kp->appSamplingCallback ) {
2754 kp->appSamplingCallback( chan ) ;
2755 }
2756 }
2757
KPADSetSamplingCallback(s32 chan,WPADSamplingCallback callback)2758 void KPADSetSamplingCallback( s32 chan, WPADSamplingCallback callback )
2759 {
2760 inside_kpads[ chan ].appSamplingCallback = callback ;
2761 }
2762
2763 /*******************************************************************************
2764 Get WPAD-format data
2765 *******************************************************************************/
KPADGetUnifiedWpadStatus(s32 chan,KPADUnifiedWpadStatus * dst,u32 count)2766 void KPADGetUnifiedWpadStatus( s32 chan, KPADUnifiedWpadStatus *dst, u32 count )
2767 {
2768 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2769 KPADUnifiedWpadStatus *uwp ;
2770 BOOL enabled ;
2771 u32 idx ;
2772
2773 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2774
2775 if ( count > kp->uniRingBufExLen + KPAD_RING_BUFS ) {
2776 count = kp->uniRingBufExLen + KPAD_RING_BUFS ;
2777 }
2778
2779 enabled = OSDisableInterrupts() ;
2780
2781 idx = kp->bufIdx ;
2782 while ( count-- ) {
2783 if ( idx == 0 ) {
2784 idx = kp->uniRingBufExLen + KPAD_RING_BUFS - 1 ;
2785 } else {
2786 idx-- ;
2787 }
2788 // <== latest ... oldest ==>
2789 // dst[0] dst[1] ... dst[KPAD_RING_BUFS - 1]
2790 if ( idx >= KPAD_RING_BUFS ) {
2791 uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ;
2792 } else {
2793 uwp = &kp->uniRingBuf[ idx ] ;
2794 }
2795 if ( WPADGetStatus() != WPAD_STATE_SETUP ) {
2796 uwp->u.core.err = WPAD_ERR_INVALID ;
2797 }
2798 memcpy( dst, uwp, sizeof(KPADUnifiedWpadStatus) ) ;
2799 dst++ ;
2800 }
2801
2802 (void)OSRestoreInterrupts( enabled ) ;
2803 }
2804
2805 /*******************************************************************************
2806 Clamping method for the Control Stick
2807 *******************************************************************************/
KPADEnableStickCrossClamp(void)2808 void KPADEnableStickCrossClamp( void )
2809 {
2810 kp_stick_clamp_cross = TRUE ;
2811 }
2812
KPADDisableStickCrossClamp(void)2813 void KPADDisableStickCrossClamp( void )
2814 {
2815 kp_stick_clamp_cross = FALSE ;
2816 }
2817
2818 /*******************************************************************************
2819 Angle correction for Nunchuk acceleration
2820 *******************************************************************************/
KPADSetReviseMode(s32 chan,BOOL sw)2821 void KPADSetReviseMode( s32 chan, BOOL sw )
2822 {
2823 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2824
2825 kp->fsAccRevise = (u8)sw ;
2826 }
2827
KPADReviseAcc(Vec * acc)2828 f32 KPADReviseAcc( Vec *acc )
2829 {
2830 MTXMultVec( kp_fs_rot, acc, acc ) ;
2831
2832 return kp_fs_revise_deg ; // Return the correction angle (in degrees).
2833 }
2834
KPADGetReviseAngle(void)2835 f32 KPADGetReviseAngle( void )
2836 {
2837 return kp_fs_revise_deg ; // Return the correction angle (in degrees).
2838 }
2839
2840 /*******************************************************************************
2841 Control for all calculation methods
2842 *******************************************************************************/
KPADSetPosPlayMode(s32 chan,KPADPlayMode mode)2843 void KPADSetPosPlayMode( s32 chan, KPADPlayMode mode )
2844 {
2845 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2846 inside_kpads[ chan ].pos_play_mode = mode ;
2847 }
2848
KPADSetHoriPlayMode(s32 chan,KPADPlayMode mode)2849 void KPADSetHoriPlayMode( s32 chan, KPADPlayMode mode )
2850 {
2851 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2852 inside_kpads[ chan ].hori_play_mode = mode ;
2853 }
2854
KPADSetDistPlayMode(s32 chan,KPADPlayMode mode)2855 void KPADSetDistPlayMode( s32 chan, KPADPlayMode mode )
2856 {
2857 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2858 inside_kpads[ chan ].dist_play_mode = mode ;
2859 }
2860
KPADSetAccPlayMode(s32 chan,KPADPlayMode mode)2861 void KPADSetAccPlayMode( s32 chan, KPADPlayMode mode )
2862 {
2863 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2864 inside_kpads[ chan ].acc_play_mode = mode ;
2865 }
2866
KPADGetPosPlayMode(s32 chan)2867 KPADPlayMode KPADGetPosPlayMode( s32 chan )
2868 {
2869 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2870 return ( inside_kpads[ chan ].pos_play_mode ) ;
2871 }
2872
KPADGetHoriPlayMode(s32 chan)2873 KPADPlayMode KPADGetHoriPlayMode( s32 chan )
2874 {
2875 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2876 return ( inside_kpads[ chan ].hori_play_mode ) ;
2877 }
2878
KPADGetDistPlayMode(s32 chan)2879 KPADPlayMode KPADGetDistPlayMode( s32 chan )
2880 {
2881 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2882 return ( inside_kpads[ chan ].dist_play_mode ) ;
2883 }
2884
KPADGetAccPlayMode(s32 chan)2885 KPADPlayMode KPADGetAccPlayMode( s32 chan )
2886 {
2887 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2888 return ( inside_kpads[ chan ].acc_play_mode ) ;
2889 }
2890
2891 /*******************************************************************************
2892 Button processing method controls
2893 *******************************************************************************/
KPADSetButtonProcMode(s32 chan,u8 mode)2894 void KPADSetButtonProcMode( s32 chan, u8 mode )
2895 {
2896 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2897 inside_kpads[ chan ].btnProcMode = mode ;
2898 }
2899
KPADGetButtonProcMode(s32 chan)2900 u8 KPADGetButtonProcMode( s32 chan )
2901 {
2902 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2903 return ( inside_kpads[ chan ].btnProcMode ) ;
2904 }
2905