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