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-2006 Nintendo. All rights reserved.
10
11 These coded instructions, statements, and computer programs contain
12 proprietary information of Nintendo of America Inc. and/or Nintendo
13 Company Ltd., and are protected by Federal copyright law. They may
14 not be disclosed to third parties or copied or duplicated in any form,
15 in whole or in part, without the prior written consent of Nintendo.
16 *---------------------------------------------------------------------------*/
17
18 #include "KPADinside.h"
19
20 #include <revolution.h>
21 #include <math.h>
22 #include <string.h>
23 #include <stddef.h>
24
25 #include <revolution/revodefs.h>
26 REVOLUTION_LIB_VERSION(KPAD);
27
28
29 /*******************************************************
30 VARIABLE
31 *******************************************************/
32 //----- DPD calibration default value
33 static Vec2 icenter_org = { 0.000f, 0.000f } ; // Center coordinate of two marks in CMOS
34 static f32 idist_org = 1.000f ; // Distance at calibration (in meters)
35 static Vec2 iaccXY_nrm_hori = { 0.000f,-1.000f } ; // XY acceleration direction 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 extremes (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 ; // Stationary state determination radius
42 f32 kp_ah_circle_pw = 0.060f ; // Stationary state determination tracking level
43 u16 kp_ah_circle_ct = 100 ; // Stationary state determination count
44 BOOL kp_stick_clamp_cross = FALSE ; // Perform cross clamping
45
46 //----- Values determined to be an error.
47 f32 kp_err_outside_frame = 0.050f ; // Surrounding area width where center of mass coordinate will be invalid (not all surrounding lights are necessarily shown)
48 f32 kp_err_dist_min ; // Minimum operational distance automatic calculation (in meters)
49 f32 kp_err_dist_max = 3.000f ; // Maximum operational distance (in meters)
50 f32 kp_err_dist_speed = 0.040f ; // Accepted range of distance change (in meters)
51 f32 kp_err_first_inpr = 0.900f ; // Internal product of acceleration tilt and object tilt when selecting two points for the first time
52 f32 kp_err_next_inpr = 0.900f ; // Accepted range of tilt change (internal product value)
53 f32 kp_err_acc_inpr = 0.900f ; // Accepted range of acceleration tilt and internal product value at stationary state
54 f32 kp_err_up_inpr = 0.700f ; // Accepted internal product range 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 coordinate is 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 ; // Constant
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
79 //----- KPAD
80 KPADInsideStatus inside_kpads[ WPAD_MAX_CONTROLLERS ] ;
81
82 //----- Zero vector
83 static Vec2 Vec2_0 = { 0.0f, 0.0f } ;
84
85 //----- For correction of Nunchuk acceleration
86 static Mtx kp_fs_rot ; // Rotation matrix
87 static f32 kp_fs_revise_deg = 24.0f ; // Correction angle (in degrees)
88
89 static void KPADiSamplingCallback ( s32 chan ) ;
90 static void KPADiControlDpdCallback( s32 chan, s32 result ) ;
91
92 static void *get_ring_buffer_by_kpad1_style( s32 chan, void *buf, u32 size ) ;
93
94 /*******************************************************************************
95 Analog data clamp setting
96 *******************************************************************************/
KPADSetFSStickClamp(s8 min,s8 max)97 void KPADSetFSStickClamp( s8 min, s8 max )
98 {
99 kp_fs_fstick_min = (s32)min ;
100 kp_fs_fstick_max = (s32)max ;
101 }
102
103 // These APIs are not recommended.
104 // Please use KPADGetUnifiedWpadStatus() instead.
105 // These are provided for compatibility with KPAD1. But not 100% compatible.
106
107 /*******************************************************************************
108 Obtain the ring buffer of WPADStatus
109 *******************************************************************************/
KPADGetWPADRingBuffer(s32 chan)110 WPADStatus *KPADGetWPADRingBuffer( s32 chan )
111 {
112 static WPADStatus status[ KPAD_RING_BUFS ] ;
113
114 return (WPADStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_CORE ) ;
115 }
116
KPADGetWPADFSRingBuffer(s32 chan)117 WPADFSStatus *KPADGetWPADFSRingBuffer( s32 chan )
118 {
119 static WPADFSStatus status[ KPAD_RING_BUFS ] ;
120
121 return (WPADFSStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_FREESTYLE ) ;
122 }
123
KPADGetWPADCLRingBuffer(s32 chan)124 WPADCLStatus *KPADGetWPADCLRingBuffer( s32 chan )
125 {
126 static WPADCLStatus status[ KPAD_RING_BUFS ] ;
127
128 return (WPADCLStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_CLASSIC ) ;
129 }
130
get_ring_buffer_by_kpad1_style(s32 chan,void * buf,u32 dev)131 static void *get_ring_buffer_by_kpad1_style( s32 chan, void *buf, u32 dev )
132 {
133 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
134 BOOL enabled ;
135 s32 idx1 ;
136 s32 idx2 ;
137 u32 count ;
138 u32 size ;
139 u32 type ;
140
141 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
142
143 switch ( dev ) {
144 case WPAD_DEV_CORE :
145 size = sizeof( WPADStatus ) ;
146 break ;
147
148 case WPAD_DEV_FREESTYLE :
149 size = sizeof( WPADFSStatus ) ;
150 break ;
151
152 case WPAD_DEV_CLASSIC :
153 size = sizeof( WPADCLStatus ) ;
154 break ;
155
156 default :
157 return buf ;
158 }
159
160 enabled = OSDisableInterrupts() ;
161 idx1 = kp->bufIdx - 1 ;
162 idx2 = (s32)WPADGetLatestIndexInBuf( chan ) ;
163 for ( count = 0; count < KPAD_RING_BUFS; count++ ) {
164 if ( idx1 < 0 ) {
165 idx1 = KPAD_RING_BUFS - 1 ;
166 }
167 if ( idx2 < 0 ) {
168 idx2 = KPAD_RING_BUFS - 1 ;
169 }
170
171 switch( kp->uniRingBuf[idx1].u.core.dev ) {
172 case WPAD_DEV_CORE:
173 case WPAD_DEV_FUTURE:
174 case WPAD_DEV_NOT_SUPPORTED:
175 case WPAD_DEV_UNKNOWN:
176 type = WPAD_DEV_CORE;
177 break;
178
179 case WPAD_DEV_FREESTYLE:
180 type = WPAD_DEV_FREESTYLE;
181 break;
182
183 case WPAD_DEV_CLASSIC:
184 type = WPAD_DEV_CLASSIC;
185 break;
186
187 default:
188 // Unreachable here.
189 type = WPAD_DEV_UNKNOWN;
190 break;
191 }
192
193 if ( type == dev ) {
194 if ( WPADGetStatus() != WPAD_STATE_SETUP ) {
195 kp->uniRingBuf[idx1].u.core.err = WPAD_ERR_INVALID ;
196 }
197 memcpy((u8 *)buf + idx2 * size,
198 &kp->uniRingBuf[ idx1 ].u,
199 size) ;
200 }
201 idx1-- ;
202 idx2-- ;
203 }
204 (void)OSRestoreInterrupts( enabled ) ;
205
206 return buf ;
207 }
208
209
210
211 /*******************************************************************************
212 Configure the repeat rate of the buttons
213 *******************************************************************************/
KPADSetBtnRepeat(s32 chan,f32 delay_sec,f32 pulse_sec)214 void KPADSetBtnRepeat( s32 chan, f32 delay_sec, f32 pulse_sec )
215 {
216 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
217
218 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
219 if ( pulse_sec ) {
220 //----- Set repeat flag setting
221 kp->btn_repeat_delay = (u16)(s32)( delay_sec * 200.0f + 0.5f ) ;
222 kp->btn_repeat_pulse = (u16)(s32)( pulse_sec * 200.0f + 0.5f ) ;
223 } else {
224 //----- No repeat flag setting
225 kp->btn_repeat_delay = KPAD_BTN_NO_RPT_DELAY ;
226 kp->btn_repeat_pulse = 0 ;
227 }
228
229 //----- Reset
230 kp->btn_repeat_time = 0 ;
231 kp->btn_repeat_next = kp->btn_repeat_delay ;
232 kp->btn_cl_repeat_time = 0 ;
233 kp->btn_cl_repeat_next = kp->btn_repeat_delay ;
234 }
235
236
237 /*******************************************************************************
238 Set marker placement interval (in meters)
239 *******************************************************************************/
KPADSetObjInterval(f32 interval)240 void KPADSetObjInterval( f32 interval )
241 {
242 BOOL enabled = OSDisableInterrupts() ;
243
244 kp_obj_interval = interval ;
245
246 //----- DPD operation minimum distance (so that the length between marks will be half that of lens diameter)
247 kp_err_dist_min = interval / KPAD_CMOS_HFOV_TAN ;
248
249 //----- Distance calculation constants
250 kp_dist_vv1 = interval / KPAD_CMOS_HFOV_TAN ;
251
252 (void)OSRestoreInterrupts( enabled ) ;
253 }
254
255
256 /*******************************************************************************
257 Set parameters
258 *******************************************************************************/
KPADSetPosParam(s32 chan,f32 play_radius,f32 sensitivity)259 void KPADSetPosParam( s32 chan, f32 play_radius, f32 sensitivity )
260 {
261 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
262 inside_kpads[ chan ].pos_play_radius = play_radius ;
263 inside_kpads[ chan ].pos_sensitivity = sensitivity ;
264 }
265
KPADSetHoriParam(s32 chan,f32 play_radius,f32 sensitivity)266 void KPADSetHoriParam( s32 chan, f32 play_radius, f32 sensitivity )
267 {
268 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) );
269 inside_kpads[ chan ].hori_play_radius = play_radius ;
270 inside_kpads[ chan ].hori_sensitivity = sensitivity ;
271 }
272
KPADSetDistParam(s32 chan,f32 play_radius,f32 sensitivity)273 void KPADSetDistParam( s32 chan, f32 play_radius, f32 sensitivity )
274 {
275 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
276 inside_kpads[ chan ].dist_play_radius = play_radius ;
277 inside_kpads[ chan ].dist_sensitivity = sensitivity ;
278 }
279
KPADSetAccParam(s32 chan,f32 play_radius,f32 sensitivity)280 void KPADSetAccParam( s32 chan, f32 play_radius, f32 sensitivity )
281 {
282 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
283 inside_kpads[ chan ].acc_play_radius = play_radius ;
284 inside_kpads[ chan ].acc_sensitivity = sensitivity ;
285 }
286
287 /*******************************************************************************
288 Get parameters
289 *******************************************************************************/
KPADGetPosParam(s32 chan,f32 * play_radius,f32 * sensitivity)290 void KPADGetPosParam( s32 chan, f32 *play_radius, f32 *sensitivity )
291 {
292 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
293 *play_radius = inside_kpads[ chan ].pos_play_radius ;
294 *sensitivity = inside_kpads[ chan ].pos_sensitivity ;
295 }
296
KPADGetHoriParam(s32 chan,f32 * play_radius,f32 * sensitivity)297 void KPADGetHoriParam( s32 chan, f32 *play_radius, f32 *sensitivity )
298 {
299 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
300 *play_radius = inside_kpads[ chan ].hori_play_radius ;
301 *sensitivity = inside_kpads[ chan ].hori_sensitivity ;
302 }
303
KPADGetDistParam(s32 chan,f32 * play_radius,f32 * sensitivity)304 void KPADGetDistParam( s32 chan, f32 *play_radius, f32 *sensitivity )
305 {
306 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
307 *play_radius = inside_kpads[ chan ].dist_play_radius ;
308 *sensitivity = inside_kpads[ chan ].dist_sensitivity ;
309 }
310
KPADGetAccParam(s32 chan,f32 * play_radius,f32 * sensitivity)311 void KPADGetAccParam( s32 chan, f32 *play_radius, f32 *sensitivity )
312 {
313 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
314 *play_radius = inside_kpads[ chan ].acc_play_radius ;
315 *sensitivity = inside_kpads[ chan ].acc_sensitivity ;
316 }
317
318
319 /*******************************************************************************
320 Obtain an easy-to-use scale value from the calibrated center position
321 *******************************************************************************/
calc_dpd2pos_scale(KPADInsideStatus * kp)322 static void calc_dpd2pos_scale( KPADInsideStatus *kp )
323 {
324 f32 scale ;
325 f32 sx,sy ;
326
327 //----- Movable distance of the controller in the vertical and horizontal direction
328 sx = 1.0f ; // Horizontal
329 sy = (f32)KPAD_DPD_RESO_WY / (f32)KPAD_DPD_RESO_WX ; // Vertical
330
331 //----- Longest movable distance of the controller
332 scale = sqrtf( sx * sx + sy * sy ) ;// Diagonal
333
334 //----- Correct the horizontal movable distance
335 if ( kp->center_org.x < 0.0f ) {
336 sx += kp->center_org.x ;
337 } else {
338 sx -= kp->center_org.x ;
339 }
340
341 //----- Correct the vertical movable distance
342 if ( kp->center_org.y < 0.0f ) {
343 sy += kp->center_org.y ;
344 } else {
345 sy -= kp->center_org.y ;
346 }
347
348 //----- A scale that will cover the longest distance of the smaller of the range of movable distance.
349 kp->dpd2pos_scale = scale / ( ( sx < sy ) ? sx : sy ) ;
350 }
351
352
353 /*******************************************************************************
354 Initialize the KPAD value
355 *******************************************************************************/
reset_kpad(KPADInsideStatus * kp)356 static void reset_kpad( KPADInsideStatus *kp )
357 {
358 KPADObject *op ;
359 KPADStatus *sp = &kp->status ;
360 KPADEXStatus *ep = &kp->status.ex_status ;
361
362 kp->resetReq = FALSE ;
363
364 //----- Recalculate constant
365 kp->kobj_frame_min.x = -1.0f + kp_err_outside_frame ;
366 kp->kobj_frame_max.x = 1.0f - kp_err_outside_frame ;
367 kp->kobj_frame_min.y = -((f32)KPAD_DPD_RESO_WY / KPAD_DPD_RESO_WX) + kp_err_outside_frame ;
368 kp->kobj_frame_max.y = ((f32)KPAD_DPD_RESO_WY / KPAD_DPD_RESO_WX) - kp_err_outside_frame ;
369
370 kp->err_dist_speed_1 = 1.0f / kp_err_dist_speed ;
371 kp->err_dist_speedM_1 = -1.0f / kp_err_dist_speed ;
372 kp->ah_circle_radius2 = kp_ah_circle_radius * kp_ah_circle_radius ;
373
374 kp->err_dist_min = kp_err_dist_min ;
375 kp->dist_vv1 = kp_dist_vv1 ;
376
377 //----- Clear button information
378 sp->hold = sp->trig = sp->release = 0x00000000 ;
379 kp->btn_repeat_time = 0 ;
380 kp->btn_repeat_next = kp->btn_repeat_delay ;
381
382 //----- Clear DPD information
383 sp->dpd_valid_fg = 0 ; // Disabled
384 kp->dpd_valid2_ct = 0 ;
385
386 sp->pos = sp->vec = Vec2_0 ;
387 sp->speed = 0.0f ;
388
389 sp->horizon.x = kp->acc_horizon.x = kp->obj_horizon.x = 1.0f ;
390 sp->horizon.y = kp->acc_horizon.y = kp->obj_horizon.y = 0.0f ;
391 sp->hori_vec = Vec2_0 ;
392 sp->hori_speed = 0.0f ;
393
394 sp->acc_vertical.x = 1.0f ;
395 sp->acc_vertical.y = 0.0f ;
396
397 sp->dist = kp->dist_org ;
398 sp->dist_vec = sp->dist_speed = 0.0f ;
399
400 kp->sec_dist = sp->dist ;
401 kp->sec_length = kp->trust_sec_length = kp->dist_vv1 / kp->sec_dist ;
402 kp->sec_nrm = kp->sec_nrm_hori ;
403
404 //----- Clear acceleration information
405 sp->acc.x = sp->acc.z = 0.0f ;
406 sp->acc.y = -1.0f ;
407 sp->acc_value = 1.0f ;
408 sp->acc_speed = 0.0f ;
409 kp->hard_acc = sp->acc ;
410
411 kp->ah_circle_pos = kp->acc_horizon ;
412 kp->ah_circle_ct = kp_ah_circle_ct ;
413
414 //----- Clear individual object information
415 kp->valid_objs = 0 ;
416
417 op = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
418 do {
419 op->error_fg = -1 ; // Not displayed
420 } while ( --op >= kp->kobj_sample ) ;
421
422 op = &kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ] ;
423 do {
424 op->error_fg = -1 ; // Not displayed
425 } while ( --op >= kp->kobj_regular ) ;
426
427 //----- Other
428 kp->bufCount = 0 ; // Ring buffer unprocessed
429
430 //----- Clear extension controller information
431 kp->exResetReq = TRUE ;
432 }
433
434
435 /*******************************************************************************
436 Convert the DPD coordinate to projection coordinate system
437 *******************************************************************************/
KPADGetProjectionPos(Vec2 * dst,const Vec2 * src,const Rect * projRect,f32 viRatio)438 void KPADGetProjectionPos( Vec2 *dst, const Vec2 *src, const Rect *projRect, f32 viRatio )
439 {
440 f32 projection_height = projRect->bottom - projRect->top ;
441
442 // Convert the normalized values into projection coordinates
443 (*dst).x = src->x * (projection_height / 2.0f) * 1.2f ;
444 (*dst).y = src->y * (projection_height / 2.0f) * 1.2f ;
445 // Horizontal direction pixel ratio correction
446 (*dst).x *= viRatio * 0.908 ;
447 }
448
449
450 /*******************************************************************************
451 Calibration
452 *******************************************************************************/
KPADCalibrateDPD(s32 chan)453 s32 KPADCalibrateDPD( s32 chan )
454 {
455 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
456 KPADStatus *sp = &kp->status ;
457 KPADObject *op1, *op2 ;
458 f32 f1, vx,vy ;
459
460 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
461
462 if ( kp->valid_objs != KPAD_USE_OBJECTS ) return ( kp->valid_objs ) ;
463
464 /***********************************************************************
465 Acceleration after calibration
466 ***********************************************************************/
467 vx = kp->hard_acc.x ;
468 vy = kp->hard_acc.y ;
469 f1 = sqrtf( vx * vx + vy * vy ) ;
470 if ( f1 <= 0.5f ) return (-1) ; // Abnormal acceleration
471 kp->accXY_nrm_hori.x = vx / f1 ;
472 kp->accXY_nrm_hori.y = vy / f1 ;
473
474 /***********************************************************************
475 Object location after calibration
476 ***********************************************************************/
477 //----- Determine the mark order by location
478 op1 = kp->kobj_sample ;
479 while ( op1->error_fg != 0 ) ++op1 ;
480 op2 = op1 + 1 ;
481 while ( op2->error_fg != 0 ) ++op2 ;
482
483 if ( op1->center.x < op2->center.x ) goto LABEL_cp12 ;
484 else if ( op2->center.x < op1->center.x ) goto LABEL_cp21 ;
485 else if ( op2->center.y < op1->center.y ) goto LABEL_cp21 ;
486
487 LABEL_cp12:
488 kp->kobj_regular[ 0 ] = *op1 ;
489 kp->kobj_regular[ 1 ] = *op2 ;
490 goto LABEL_cpend ;
491
492 LABEL_cp21:
493 kp->kobj_regular[ 0 ] = *op2 ;
494 kp->kobj_regular[ 1 ] = *op1 ;
495
496 LABEL_cpend:
497
498 //kp->center_org.x = ( kp->kobj_regular[0].center.x + kp->kobj_regular[1].center.x ) * 0.5f ;
499 //kp->center_org.y = ( kp->kobj_regular[0].center.y + kp->kobj_regular[1].center.y ) * 0.5f ;
500 //kp->center_org.x = kp->center_org.y = 0.0f ;
501 calc_dpd2pos_scale( kp ) ;
502
503 //----- Section direction when the controller is in horizontal position
504 vx = kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ].center.x - kp->kobj_regular[ 0 ].center.x ;
505 vy = kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ].center.y - kp->kobj_regular[ 0 ].center.y ;
506 f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not be zero
507 kp->sec_nrm_hori.x = vx * f1 ;
508 kp->sec_nrm_hori.y = vy * f1 ;
509
510 /***********************************************************************
511 Distance at calibration
512 ***********************************************************************/
513 kp->dist_org = kp->dist_vv1 * f1 ;
514
515 /***********************************************************************
516 Other
517 ***********************************************************************/
518 sp->dpd_valid_fg = 0 ; // Invalid temporarily
519
520 return ( kp->valid_objs ) ;
521 }
522
523
524 /*******************************************************************************
525 Enable Wii Remote direction correction
526 *******************************************************************************/
KPADEnableAimingMode(s32 chan)527 void KPADEnableAimingMode( s32 chan )
528 {
529 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
530 inside_kpads[ chan ].aimReq = TRUE ;
531 inside_kpads[ chan ].aimEnabled = TRUE ;
532 }
533
534
535 /*******************************************************************************
536 Disable Wii Remote direction correction
537 *******************************************************************************/
KPADDisableAimingMode(s32 chan)538 void KPADDisableAimingMode( s32 chan )
539 {
540 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) );
541 inside_kpads[ chan ].aimReq = TRUE ;
542 inside_kpads[ chan ].aimEnabled = FALSE ;
543 }
544
545
546 /*******************************************************************************
547 Calibration
548 *******************************************************************************/
KPADSetSensorHeight(s32 chan,f32 level)549 void KPADSetSensorHeight( s32 chan, f32 level )
550 {
551 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
552
553 kp->center_org.x = 0.0f ;
554 kp->center_org.y = -level ;
555 calc_dpd2pos_scale( kp ) ;
556 }
557
558
559 /*******************************************************************************
560 Digital button repeat process
561 *******************************************************************************/
calc_button_repeat(KPADInsideStatus * kp,u32 dev_type,u32 count)562 static void calc_button_repeat( KPADInsideStatus *kp, u32 dev_type, u32 count )
563 {
564 KPADStatus *sp = &kp->status ;
565 KPADEXStatus *ep = &kp->status.ex_status ;
566
567 if ( sp->trig != 0 || sp->release != 0 ) {
568 //----- Reset because the button state changed
569 kp->btn_repeat_time = 0 ;
570 kp->btn_repeat_next = kp->btn_repeat_delay ;
571
572 //----- Set flags at the beginning of push (only when repeat is set)
573 if ( sp->trig && kp->btn_repeat_pulse ) {
574 sp->hold |= KPAD_BUTTON_RPT ;
575 }
576 } else if ( sp->hold != 0 ) {
577 //----- Forward time because the button is pushed and the state is not changing
578 kp->btn_repeat_time += count ;
579 if ( kp->btn_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) {
580 kp->btn_repeat_time -= KPAD_BTN_NO_RPT_DELAY ;
581 }
582
583 //----- Set flag at the repeat time
584 if ( kp->btn_repeat_time >= kp->btn_repeat_next ) {
585 sp->hold |= KPAD_BUTTON_RPT ;
586
587 //----- Configure the next repeat time
588 kp->btn_repeat_next += kp->btn_repeat_pulse ;
589
590 //----- If the time has exceeded the range here, make it loop
591 if ( kp->btn_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) {
592 kp->btn_repeat_time -= KPAD_BTN_RPT_TIME_MAX ;
593 kp->btn_repeat_next -= KPAD_BTN_RPT_TIME_MAX ;
594 }
595 }
596 }
597
598 if (dev_type == WPAD_DEV_CLASSIC) {
599 if ( ep->cl.trig != 0 || ep->cl.release != 0 ) {
600 //----- Reset because the button state changed
601 kp->btn_cl_repeat_time = 0 ;
602 kp->btn_cl_repeat_next = kp->btn_repeat_delay ;
603
604 //----- Set flags at the beginning of push (only when repeat is set)
605 if ( ep->cl.trig && kp->btn_repeat_pulse ) {
606 ep->cl.hold |= KPAD_BUTTON_RPT ;
607 }
608 } else if ( ep->cl.hold != 0 ) {
609 //----- Forward time because the button is pushed and the state is not changing
610 kp->btn_cl_repeat_time += count ;
611 if ( kp->btn_cl_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) {
612 kp->btn_cl_repeat_time -= KPAD_BTN_NO_RPT_DELAY ;
613 }
614
615 //----- Set flag at the repeat time
616 if ( kp->btn_cl_repeat_time >= kp->btn_cl_repeat_next ) {
617 ep->cl.hold |= KPAD_BUTTON_RPT ;
618
619 //----- Configure the next repeat time
620 kp->btn_cl_repeat_next += kp->btn_repeat_pulse ;
621
622 //----- If the time has exceeded the range here, make it loop
623 if ( kp->btn_cl_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) {
624 kp->btn_cl_repeat_time -= KPAD_BTN_RPT_TIME_MAX ;
625 kp->btn_cl_repeat_next -= KPAD_BTN_RPT_TIME_MAX ;
626 }
627 }
628 }
629 }
630 }
631
632
633 /*******************************************************************************
634 KPAD button information load process
635 *******************************************************************************/
read_kpad_button(KPADInsideStatus * kp,u32 dev_type,u32 count,u32 core,u32 fs,u32 cl)636 static void read_kpad_button( KPADInsideStatus *kp, u32 dev_type, u32 count, u32 core, u32 fs, u32 cl)
637 {
638 KPADStatus *sp = &kp->status ;
639 KPADEXStatus *ep = &kp->status.ex_status ;
640 u32 old_fg, change_fg ;
641 u32 cl_old_fg, cl_change_fg ;
642
643 //----- Store the previous value
644 old_fg = sp->hold & KPAD_BUTTON_MASK ;
645
646 //----- Load new value
647 sp->hold = ( core & (KPAD_BUTTON_MASK & ~(WPAD_BUTTON_Z | WPAD_BUTTON_C)) ) |
648 ( fs & (WPAD_BUTTON_Z | WPAD_BUTTON_C) ) ;
649
650 //----- Button state process
651 change_fg = sp->hold ^ old_fg ; // Changed button
652 sp->trig = change_fg & sp->hold ; // Pushed button
653 sp->release = change_fg & old_fg ; // Released button
654
655 if ( dev_type == WPAD_DEV_CLASSIC ) {
656 //----- Store the previous value
657 cl_old_fg = ep->cl.hold & KPAD_BUTTON_MASK ;
658
659 //----- Load new value
660 ep->cl.hold = cl & KPAD_BUTTON_MASK ;
661
662 //----- Button state process
663 cl_change_fg = ep->cl.hold ^ cl_old_fg ;
664 ep->cl.trig = cl_change_fg & ep->cl.hold ;
665 ep->cl.release = cl_change_fg & cl_old_fg ;
666 }
667
668 //----- Repeat processing
669 calc_button_repeat( kp, dev_type, count ) ;
670 }
671
672
673 /*******************************************************************************
674 Acceleration tracking process
675 *******************************************************************************/
calc_acc(KPADInsideStatus * kp,f32 * acc,f32 acc2)676 static void calc_acc( KPADInsideStatus *kp, f32 *acc, f32 acc2 )
677 {
678 f32 f1,f2 ;
679
680
681 //----- Difference to the target
682 f2 = acc2 - *acc ;
683
684 if ( kp->acc_play_mode == KPAD_PLAY_MODE_LOOSE ) {
685 if ( f2 < 0.0f ) {
686 f1 = -f2 ;
687 } else {
688 f1 = f2 ;
689 }
690
691 //----- Tracking rate inside/outside the tolerance
692 if ( f1 >= kp->acc_play_radius ) {
693 //----- Apply 100% tracking if outside tolerance
694 f1 = 1.0f ;
695 } else {
696 //----- If inside tolerance, weaken tracking as target gets closer
697 f1 /= kp->acc_play_radius ;
698 f1 *= f1 ; // Second power
699 f1 *= f1 ; // Fourth power
700 }
701 f1 *= kp->acc_sensitivity ;
702
703 //----- Tracking
704 *acc += f1 * f2 ;
705 } else {
706 if ( f2 < -kp->acc_play_radius ) {
707 *acc += ( f2 + kp->acc_play_radius ) * kp->acc_sensitivity ;
708 } else if ( f2 > kp->acc_play_radius ) {
709 *acc += ( f2 - kp->acc_play_radius ) * kp->acc_sensitivity ;
710 }
711 }
712 }
713
714
715 /*******************************************************************************
716 Calculate controller tilt from acceleration
717 *******************************************************************************/
calc_acc_horizon(KPADInsideStatus * kp)718 static void calc_acc_horizon( KPADInsideStatus *kp )
719 {
720 f32 f1, vx,vy, ax,ay ;
721
722
723 //----- xy acceleration normalization
724 f1 = sqrtf( kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ;
725 if ( f1 == 0.0f || f1 >= 2.0f ) return ;
726 ax = kp->hard_acc.x / f1 ;
727 ay = kp->hard_acc.y / f1 ;
728
729 //----- There will be more power the closer the xy acceleration length is to one (1).
730 if ( f1 > 1.0f ) {
731 f1 = 2.0f - f1 ;
732 }
733 f1 *= f1 * kp_acc_horizon_pw ;
734
735 //----- Target tilt
736 vx = kp->accXY_nrm_hori.x * ax + kp->accXY_nrm_hori.y * ay ;
737 vy = kp->accXY_nrm_hori.y * ax - kp->accXY_nrm_hori.x * ay ;
738
739 //----- Set closer
740 ax = ( vx - kp->acc_horizon.x ) * f1 + kp->acc_horizon.x ;
741 ay = ( vy - kp->acc_horizon.y ) * f1 + kp->acc_horizon.y ;
742
743 //----- Normalization
744 f1 = sqrtf( ax * ax + ay * ay ) ;
745 if ( f1 == 0.0f ) return ;
746 kp->acc_horizon.x = ax / f1 ;
747 kp->acc_horizon.y = ay / f1 ;
748
749
750 //----- Update stationary state determination coordinate
751 kp->ah_circle_pos.x += ( kp->acc_horizon.x - kp->ah_circle_pos.x ) * kp_ah_circle_pw ;
752 kp->ah_circle_pos.y += ( kp->acc_horizon.y - kp->ah_circle_pos.y ) * kp_ah_circle_pw ;
753
754 vx = kp->acc_horizon.x - kp->ah_circle_pos.x ;
755 vy = kp->acc_horizon.y - kp->ah_circle_pos.y ;
756 if ( vx*vx + vy*vy <= kp->ah_circle_radius2 ) {
757 if ( kp->ah_circle_ct ) -- kp->ah_circle_ct ;
758 } else {
759 kp->ah_circle_ct = kp_ah_circle_ct ;
760 }
761 }
762
calc_acc_vertical(KPADInsideStatus * kp)763 static void calc_acc_vertical( KPADInsideStatus *kp )
764 {
765 KPADStatus *sp = &kp->status ;
766 f32 f1,f2, ax,ay ;
767
768
769 //----- Target tilt
770 ax = sqrtf( f2 = kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ;
771 ay = - kp->hard_acc.z ;
772 f1 = sqrtf( f2 + ay * ay ) ;
773 if ( f1 == 0.0f || f1 >= 2.0f ) return ;
774 ax /= f1 ;
775 ay /= f1 ;
776
777 //----- There will be more power the closer the acceleration length is to one (1).
778 if ( f1 > 1.0f ) {
779 f1 = 2.0f - f1 ;
780 }
781 f1 *= f1 * kp_acc_horizon_pw ;
782
783 //----- Set closer
784 ax = ( ax - sp->acc_vertical.x ) * f1 + sp->acc_vertical.x ;
785 ay = ( ay - sp->acc_vertical.y ) * f1 + sp->acc_vertical.y ;
786
787 //----- Normalization
788 f1 = sqrtf( ax * ax + ay * ay ) ;
789 if ( f1 == 0.0f ) return ;
790 sp->acc_vertical.x = ax / f1 ;
791 sp->acc_vertical.y = ay / f1 ;
792 }
793
794
795 /*******************************************************************************
796 KPAD acceleration information load process
797 *******************************************************************************/
clamp_acc(f32 acc,f32 clamp)798 static f32 clamp_acc( f32 acc, f32 clamp )
799 {
800 if ( acc < 0.0f ) {
801 if ( acc < -clamp ) return ( -clamp ) ;
802 } else {
803 if ( acc > clamp ) return ( clamp ) ;
804 }
805 return ( acc ) ;
806 }
807
read_kpad_acc(KPADInsideStatus * kp,KPADUnifiedWpadStatus * uwp)808 static void read_kpad_acc( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp )
809 {
810 KPADStatus *sp = &kp->status ;
811 Vec fsrc ;
812 Vec vec ;
813
814 switch ( uwp->fmt ) {
815 case WPAD_FMT_CORE_ACC :
816 case WPAD_FMT_CORE_ACC_DPD :
817 case WPAD_FMT_FREESTYLE_ACC :
818 case WPAD_FMT_FREESTYLE_ACC_DPD :
819 case WPAD_FMT_CLASSIC_ACC :
820 case WPAD_FMT_CLASSIC_ACC_DPD :
821 // core ACC is OK
822 break ;
823
824 default :
825 return ;
826 }
827
828 //----- Update raw value
829 kp->hard_acc.x = clamp_acc( (f32)(s32)-uwp->u.core.accX * kp->acc_scale_x, kp_rm_acc_max ) ;
830 kp->hard_acc.y = clamp_acc( (f32)(s32)-uwp->u.core.accZ * kp->acc_scale_z, kp_rm_acc_max ) ;
831 kp->hard_acc.z = clamp_acc( (f32)(s32) uwp->u.core.accY * kp->acc_scale_y, kp_rm_acc_max ) ;
832
833 //----- Temporary save
834 vec = sp->acc ;
835
836 //----- Acceleration tracking process for the application
837 calc_acc( kp, &sp->acc.x, kp->hard_acc.x ) ;
838 calc_acc( kp, &sp->acc.y, kp->hard_acc.y ) ;
839 calc_acc( kp, &sp->acc.z, kp->hard_acc.z ) ;
840 sp->acc_value = sqrtf( sp->acc.x * sp->acc.x + sp->acc.y * sp->acc.y + sp->acc.z * sp->acc.z ) ;
841
842 //----- Acceleration change for the application
843 vec.x -= sp->acc.x ;
844 vec.y -= sp->acc.y ;
845 vec.z -= sp->acc.z ;
846 sp->acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ;
847
848 //----- Calculate controller tilt from the raw acceleration
849 calc_acc_horizon( kp ) ;
850 calc_acc_vertical( kp ) ;
851
852
853 /***********************************************************************
854 Load Nunchuk unit acceleration
855 ***********************************************************************/
856 if ( uwp->u.fs.err != WPAD_ERR_NONE ||
857 uwp->u.fs.dev != WPAD_DEV_FREESTYLE ||
858 (uwp->fmt != WPAD_FMT_FREESTYLE_ACC &&
859 uwp->fmt != WPAD_FMT_FREESTYLE_ACC_DPD) ) {
860 return ;
861 }
862
863 fsrc.x = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccX * kp->fs_acc_scale_x, kp_fs_acc_max ) ;
864 fsrc.y = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccZ * kp->fs_acc_scale_z, kp_fs_acc_max ) ;
865 fsrc.z = clamp_acc( (f32)(s32) uwp->u.fs.fsAccY * kp->fs_acc_scale_y, kp_fs_acc_max ) ;
866
867 if ( kp->fsAccRevise ) {
868 MTXMultVec( kp_fs_rot, &fsrc, &fsrc ) ;
869 }
870
871 //----- Temporary save
872 vec = sp->ex_status.fs.acc ;
873
874 //----- Acceleration tracking process for the application
875 calc_acc( kp, &sp->ex_status.fs.acc.x, fsrc.x ) ;
876 calc_acc( kp, &sp->ex_status.fs.acc.y, fsrc.y ) ;
877 calc_acc( kp, &sp->ex_status.fs.acc.z, fsrc.z ) ;
878 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 ) ;
879
880 //----- Acceleration change for the application
881 vec.x -= sp->ex_status.fs.acc.x ;
882 vec.y -= sp->ex_status.fs.acc.y ;
883 vec.z -= sp->ex_status.fs.acc.z ;
884 sp->ex_status.fs.acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ;
885 }
886
887
888 /*******************************************************************************
889 Change the WPAD object to KPAD
890 *******************************************************************************/
get_kobj(KPADInsideStatus * kp,DPDObject * wobj_p)891 static void get_kobj( KPADInsideStatus *kp, DPDObject *wobj_p )
892 {
893 const f32 dpd_scale = 2.0f / (f32)KPAD_DPD_RESO_WX ;
894 const f32 dpd_cx = (f32)( KPAD_DPD_RESO_WX - 1 ) / (f32)KPAD_DPD_RESO_WX ;
895 const f32 dpd_cy = (f32)( KPAD_DPD_RESO_WY - 1 ) / (f32)KPAD_DPD_RESO_WX ;
896
897 KPADObject *kobj_p ;
898
899 //----- Store
900 kobj_p = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
901 do {
902 if ( wobj_p->size ) {
903 //----- Valid object
904 kobj_p->center.x = (f32)(s32)wobj_p->x * dpd_scale - dpd_cx ;
905 kobj_p->center.y = (f32)(s32)wobj_p->y * dpd_scale - dpd_cy ;
906
907 kobj_p->error_fg = 0 ; // Displayed
908 kobj_p->state_fg = 0 ; // Normal
909 } else {
910 //----- Invalid object
911 kobj_p->error_fg = -1 ; // Not displayed
912 }
913
914 -- wobj_p ;
915 } while ( --kobj_p >= kp->kobj_sample ) ;
916 }
917
918
919 /*******************************************************************************
920 Set surrounding objects to invalid
921 *******************************************************************************/
check_kobj_outside_frame(KPADInsideStatus * kp,KPADObject * kobj_t)922 static void check_kobj_outside_frame( KPADInsideStatus *kp, KPADObject *kobj_t )
923 {
924 KPADObject *kobj_p = &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
925
926 do {
927 if ( kobj_p->error_fg < 0 ) continue ;
928
929 if ( kobj_p->center.x <= kp->kobj_frame_min.x || kobj_p->center.x >= kp->kobj_frame_max.x ||
930 kobj_p->center.y <= kp->kobj_frame_min.y || kobj_p->center.y >= kp->kobj_frame_max.y ) {
931 kobj_p->error_fg |= 1 ;
932 }
933 } while ( --kobj_p >= kobj_t ) ;
934 }
935
936
937 /*******************************************************************************
938 Set objects at the same coordinate to invalid
939 *******************************************************************************/
check_kobj_same_position(KPADObject * kobj_t)940 static void check_kobj_same_position( KPADObject *kobj_t )
941 {
942 KPADObject *op1, *op2 ;
943
944
945 op1 = kobj_t ;
946 do {
947 if ( op1->error_fg != 0 ) continue ;
948
949 op2 = op1 + 1 ;
950 do {
951 if ( op2->error_fg != 0 ) continue ;
952
953 if ( op1->center.x == op2->center.x && op1->center.y == op2->center.y ) {
954 op2->error_fg |= 2 ; // Set just one as error
955 }
956 } while ( ++op2 <= &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
957 } while ( ++op1 < &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
958 }
959
960
961 /*******************************************************************************
962 Calculate controller tilt from two points (return the distance from TV)
963 *******************************************************************************/
calc_horizon(KPADInsideStatus * kp,Vec2 * p1,Vec2 * p2,Vec2 * hori)964 static f32 calc_horizon( KPADInsideStatus *kp, Vec2 *p1, Vec2 *p2, Vec2 *hori )
965 {
966 f32 f1, vx,vy ;
967
968
969 vx = p2->x - p1->x ;
970 vy = p2->y - p1->y ;
971 f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not be zero
972 vx *= f1 ;
973 vy *= f1 ;
974
975 hori->x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ;
976 hori->y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ;
977
978 return ( kp->dist_vv1 * f1 ) ;
979 }
980
981
982 /*******************************************************************************
983 Select two marks for the first time
984 *******************************************************************************/
select_2obj_first(KPADInsideStatus * kp)985 static s8 select_2obj_first( KPADInsideStatus *kp )
986 {
987 KPADObject *op1,*op2, *rp1,*rp2 ;
988 Vec2 hori ;
989 f32 f1, max = kp_err_first_inpr ;
990
991 op1 = kp->kobj_sample ;
992 do {
993 if ( op1->error_fg != 0 ) continue ;
994
995 op2 = op1 + 1 ;
996 do {
997 if ( op2->error_fg != 0 ) continue ;
998
999 f1 = calc_horizon( kp, &op1->center, &op2->center, &hori ) ;
1000
1001 //----- Control distance range check
1002 if ( f1 <= kp->err_dist_min || f1 >= kp_err_dist_max ) continue ;
1003
1004 f1 = kp->acc_horizon.x * hori.x + kp->acc_horizon.y * hori.y ;
1005 if ( f1 < 0.0f ) {
1006 if ( -f1 > max ) {
1007 max = -f1 ;
1008 rp1 = op2 ;
1009 rp2 = op1 ;
1010 }
1011 } else {
1012 if ( f1 > max ) {
1013 max = f1 ;
1014 rp1 = op1 ;
1015 rp2 = op2 ;
1016 }
1017 }
1018
1019 } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1020 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1021
1022 //----- Confirmed regular mark?
1023 if ( max == kp_err_first_inpr ) return ( 0 ) ;
1024
1025 kp->kobj_regular[ 0 ] = *rp1 ;
1026 kp->kobj_regular[ 1 ] = *rp2 ;
1027
1028 return ( 2 ) ; // Two points recognition
1029 }
1030
1031
1032 /*******************************************************************************
1033 Select two marks using only the interval information subsequently
1034 *******************************************************************************/
select_2obj_continue(KPADInsideStatus * kp)1035 static s8 select_2obj_continue( KPADInsideStatus *kp )
1036 {
1037 KPADObject *op1,*op2, *rp1,*rp2 ;
1038 Vec2 nrm ;
1039 s32 rev_fg ;
1040 f32 f1,f2, vx,vy, min = 2.0f ;
1041
1042
1043 //----- Find two points closest to the last tilt and distance
1044 op1 = kp->kobj_sample ;
1045 do {
1046 if ( op1->error_fg != 0 ) continue ;
1047
1048 op2 = op1 + 1 ;
1049 do {
1050 if ( op2->error_fg != 0 ) continue ;
1051
1052 //----- Direction calculation
1053 vx = op2->center.x - op1->center.x ;
1054 vy = op2->center.y - op1->center.y ;
1055 f1 = 1.0f / sqrtf( vx*vx + vy*vy ) ; // Should not be zero
1056 nrm.x = vx * f1 ;
1057 nrm.y = vy * f1 ;
1058
1059 //----- Control distance range check
1060 f1 *= kp->dist_vv1 ; // Distance
1061 if ( f1 <= kp->err_dist_min || f1 >= kp_err_dist_max ) continue ;
1062
1063 //----- Distance change check
1064 f1 -= kp->sec_dist ;
1065 if ( f1 < 0.0f ) {
1066 f1 *= kp->err_dist_speedM_1 ;
1067 } else {
1068 f1 *= kp->err_dist_speed_1 ;
1069 }
1070 if ( f1 >= 1.0f ) continue ; // Distance error rate
1071
1072 //----- Tilt change check
1073 f2 = kp->sec_nrm.x * nrm.x + kp->sec_nrm.y * nrm.y ;
1074 if ( f2 < 0.0f ) {
1075 f2 = -f2 ;
1076 rev_fg = 1 ; // Handle with direction inverted (op2 -> op1)
1077 } else {
1078 rev_fg = 0 ; // Handle as is (op1 -> op2)
1079 }
1080 if ( f2 <= kp_err_next_inpr ) continue ;
1081 f2 = ( 1.0f - f2 ) / ( 1.0f - kp_err_next_inpr ) ; // Tilt error rate
1082
1083 //----- Record the object with minimum error
1084 f1 += f2 ; // Determine through the sum of distance error rate and tilt error rate
1085 if ( f1 < min ) {
1086 min = f1 ;
1087 if ( rev_fg ) {
1088 rp1 = op2 ;
1089 rp2 = op1 ;
1090 } else {
1091 rp1 = op1 ;
1092 rp2 = op2 ;
1093 }
1094 }
1095
1096 } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1097 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1098
1099 //----- Confirmed regular mark?
1100 if ( min == 2.0f ) return ( 0 ) ;
1101
1102 kp->kobj_regular[ 0 ] = *rp1 ;
1103 kp->kobj_regular[ 1 ] = *rp2 ;
1104
1105 return ( 2 ) ; // Two points recognition
1106 }
1107
1108
1109 /*******************************************************************************
1110 Select one mark for the first time
1111 *******************************************************************************/
select_1obj_first(KPADInsideStatus * kp)1112 static s8 select_1obj_first( KPADInsideStatus *kp )
1113 {
1114 KPADObject *op1 ;
1115 f32 vx,vy ;
1116 Vec2 p1,p2 ;
1117
1118
1119 //----- Determine the section direction
1120 vx = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ;
1121 vy = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ;
1122
1123 //----- Determine the section vector
1124 vx *= kp->trust_sec_length ;
1125 vy *= kp->trust_sec_length ;
1126
1127 //----- Search for a point where the expected point is outside
1128 op1 = kp->kobj_sample ;
1129 do {
1130 if ( op1->error_fg != 0 ) continue ;
1131
1132 p1.x = op1->center.x - vx ; // Expected point to the left
1133 p1.y = op1->center.y - vy ;
1134 p2.x = op1->center.x + vx ; // Expected point to the right
1135 p2.y = op1->center.y + vy ;
1136
1137 if ( p1.x <= kp->kobj_frame_min.x || p1.x >= kp->kobj_frame_max.x ||
1138 p1.y <= kp->kobj_frame_min.y || p1.y >= kp->kobj_frame_max.y ) {
1139 //----- If the left-hand expected point is outside, the right-hand expected point needs to be inside
1140 if ( p2.x > kp->kobj_frame_min.x && p2.x < kp->kobj_frame_max.x &&
1141 p2.y > kp->kobj_frame_min.y && p2.y < kp->kobj_frame_max.y ) {
1142 //----- op1 may be right mark
1143 kp->kobj_regular[ 1 ] = *op1 ;
1144
1145 kp->kobj_regular[ 0 ].center = p1 ;
1146 kp->kobj_regular[ 0 ].error_fg = 0 ;
1147 kp->kobj_regular[ 0 ].state_fg = -1 ;
1148
1149 return ( -1 ) ; // Found one point of worry
1150 }
1151 } else {
1152 //----- If the left-hand expected point is inside, the right-hand expected point needs to be outside.
1153 if ( p2.x <= kp->kobj_frame_min.x || p2.x >= kp->kobj_frame_max.x ||
1154 p2.y <= kp->kobj_frame_min.y || p2.y >= kp->kobj_frame_max.y ) {
1155 //----- op1 may be left mark
1156 kp->kobj_regular[ 0 ] = *op1 ;
1157
1158 kp->kobj_regular[ 1 ].center = p2 ;
1159 kp->kobj_regular[ 1 ].error_fg = 0 ;
1160 kp->kobj_regular[ 1 ].state_fg = -1 ;
1161
1162 return ( -1 ) ; // Found one point of worry
1163 }
1164 }
1165
1166 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ;
1167
1168 return ( 0 ) ;
1169 }
1170
1171
1172 /*******************************************************************************
1173 Select one mark subsequently
1174 *******************************************************************************/
select_1obj_continue(KPADInsideStatus * kp)1175 static s8 select_1obj_continue( KPADInsideStatus *kp )
1176 {
1177 KPADObject *op1,*op2, *rp1,*rp2 ;
1178 f32 f1, vx,vy ;
1179 f32 min = kp_err_near_pos * kp_err_near_pos ;
1180
1181
1182 //----- Select a mark closest to the past regular mark
1183 op1 = kp->kobj_regular ;
1184 do {
1185 if ( op1->error_fg != 0 ) continue ;
1186 if ( op1->state_fg != 0 ) continue ; // No expected points
1187
1188 op2 = kp->kobj_sample ;
1189 do {
1190 if ( op2->error_fg != 0 ) continue ;
1191
1192 vx = op1->center.x - op2->center.x ;
1193 vy = op1->center.y - op2->center.y ;
1194 f1 = vx * vx + vy * vy ;
1195 if ( f1 < min ) {
1196 min = f1 ;
1197 rp1 = op1 ;
1198 rp2 = op2 ;
1199 }
1200 } while ( ++op2 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ;
1201 } while ( ++op1 < &kp->kobj_regular[ KPAD_USE_OBJECTS ] ) ;
1202
1203 //----- Confirmed regular mark?
1204 if ( min == kp_err_near_pos * kp_err_near_pos ) return ( 0 ) ;
1205
1206 *rp1 = *rp2 ;
1207
1208 //----- Calculate tilt from the acceleration
1209 kp->sec_nrm.x = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ;
1210 kp->sec_nrm.y = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ;
1211
1212 //----- Calculate expected point coordinate
1213 vx = kp->sec_length * kp->sec_nrm.x ;
1214 vy = kp->sec_length * kp->sec_nrm.y ;
1215 if ( rp1 == &kp->kobj_regular[ 0 ] ) {
1216 kp->kobj_regular[ 1 ].center.x = rp1->center.x + vx ;
1217 kp->kobj_regular[ 1 ].center.y = rp1->center.y + vy ;
1218 kp->kobj_regular[ 1 ].error_fg = 0 ;
1219 kp->kobj_regular[ 1 ].state_fg = -1 ;
1220 } else {
1221 kp->kobj_regular[ 0 ].center.x = rp1->center.x - vx ;
1222 kp->kobj_regular[ 0 ].center.y = rp1->center.y - vy ;
1223 kp->kobj_regular[ 0 ].error_fg = 0 ;
1224 kp->kobj_regular[ 0 ].state_fg = -1 ;
1225 }
1226
1227 if ( kp->status.dpd_valid_fg < 0 ) {
1228 return ( -1 ) ; // Found one point of worry
1229 } else {
1230 return ( 1 ) ; // One point recognition
1231 }
1232 }
1233
1234
1235 /*******************************************************************************
1236 Calculate controller tilt from object
1237 *******************************************************************************/
calc_obj_horizon(KPADInsideStatus * kp)1238 static void calc_obj_horizon( KPADInsideStatus *kp )
1239 {
1240 f32 f1, vx,vy ;
1241
1242
1243 vx = kp->kobj_regular[ 1 ].center.x - kp->kobj_regular[ 0 ].center.x ;
1244 vy = kp->kobj_regular[ 1 ].center.y - kp->kobj_regular[ 0 ].center.y ;
1245 kp->sec_length = sqrtf( vx * vx + vy * vy ) ; // Should not be zero
1246
1247 f1 = 1.0f / kp->sec_length ;
1248 kp->sec_dist = kp->dist_vv1 * f1 ;
1249
1250 kp->sec_nrm.x = ( vx *= f1 ) ;
1251 kp->sec_nrm.y = ( vy *= f1 ) ;
1252
1253 kp->obj_horizon.x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ;
1254 kp->obj_horizon.y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ;
1255 }
1256
1257
1258 /*******************************************************************************
1259 Update application variables
1260 *******************************************************************************/
calc_dpd_variable(KPADInsideStatus * kp,s8 valid_fg_next)1261 static void calc_dpd_variable( KPADInsideStatus *kp, s8 valid_fg_next )
1262 {
1263 KPADStatus *sp = &kp->status ;
1264 f32 f1,f2, dist ;
1265 Vec2 pos, vec ;
1266
1267
1268 if ( valid_fg_next == 0 ) {
1269 sp->dpd_valid_fg = 0 ;
1270 return ;
1271 }
1272
1273 /***********************************************************************
1274 Calculate controller tilt
1275 ***********************************************************************/
1276 //----- Calculate the target value
1277 pos.x = kp->sec_nrm_hori.x * kp->sec_nrm.x + kp->sec_nrm_hori.y * kp->sec_nrm.y ;
1278 pos.y = kp->sec_nrm_hori.y * kp->sec_nrm.x - kp->sec_nrm_hori.x * kp->sec_nrm.y ;
1279
1280 //----- Consider the tracking and tolerance for the target value
1281 if ( sp->dpd_valid_fg == 0 ) {
1282 //----- Because this is the first pointing, initialize with the given value
1283 sp->horizon = pos ;
1284 sp->hori_vec = Vec2_0 ;
1285 sp->hori_speed = 0.0f ;
1286 } else {
1287 //----- Difference to the target
1288 vec.x = pos.x - sp->horizon.x ;
1289 vec.y = pos.y - sp->horizon.y ;
1290 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ;
1291
1292 if ( kp->hori_play_mode == KPAD_PLAY_MODE_LOOSE ) {
1293 //----- Tracking rate inside/outside the tolerance
1294 if ( f1 >= kp->hori_play_radius ) {
1295 //----- Apply 100% tracking if outside tolerance
1296 f1 = 1.0f ;
1297 } else {
1298 //----- If inside tolerance, weaken tracking as target gets closer
1299 f1 /= kp->hori_play_radius ;
1300 f1 *= f1 ; // Second power
1301 f1 *= f1 ; // Fourth power
1302 }
1303 f1 *= kp->hori_sensitivity ;
1304
1305 //----- Tracking
1306 vec.x = f1 * vec.x + sp->horizon.x ;
1307 vec.y = f1 * vec.y + sp->horizon.y ;
1308 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; // Normalize because this is tilt
1309 vec.x /= f1 ;
1310 vec.y /= f1 ;
1311
1312 sp->hori_vec.x = vec.x - sp->horizon.x ;
1313 sp->hori_vec.y = vec.y - sp->horizon.y ;
1314 sp->hori_speed = sqrtf( sp->hori_vec.x * sp->hori_vec.x + sp->hori_vec.y * sp->hori_vec.y ) ;
1315
1316 sp->horizon = vec ;
1317 } else {
1318 if ( f1 > kp->hori_play_radius ) {
1319 //----- Interpolate because outside tolerance
1320 f1 = ( f1 - kp->hori_play_radius ) / f1 * kp->hori_sensitivity ;
1321 vec.x = vec.x * f1 + sp->horizon.x ;
1322 vec.y = vec.y * f1 + sp->horizon.y ;
1323 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ;
1324 vec.x /= f1 ;
1325 vec.y /= f1 ;
1326
1327 sp->hori_vec.x = vec.x - sp->horizon.x ;
1328 sp->hori_vec.y = vec.y - sp->horizon.y ;
1329 sp->hori_speed = sqrtf( sp->hori_vec.x * sp->hori_vec.x + sp->hori_vec.y * sp->hori_vec.y ) ;
1330
1331 sp->horizon = vec ;
1332 } else {
1333 //----- Does not move since it's inside tolerance
1334 sp->hori_vec = Vec2_0 ;
1335 sp->hori_speed = 0.0f ;
1336 }
1337 }
1338 }
1339
1340 /***********************************************************************
1341 Calculate the distance from the TV
1342 ***********************************************************************/
1343 //----- Calculate the target value
1344 dist = kp->dist_vv1 / kp->sec_length ;
1345
1346 //----- Consider the tracking and tolerance for the target value
1347 if ( sp->dpd_valid_fg == 0 ) {
1348 //----- Because this is the first pointing, initialize with the given value
1349 sp->dist = dist ;
1350 sp->dist_vec = 0.0f ;
1351 sp->dist_speed = 0.0f ;
1352 } else {
1353 //----- Difference to the target
1354 f2 = dist - sp->dist ;
1355 if ( f2 < 0.0f ) {
1356 f1 = -f2 ;
1357 } else {
1358 f1 = f2 ;
1359 }
1360
1361 if ( kp->dist_play_mode == KPAD_PLAY_MODE_LOOSE ) {
1362 //----- Tracking rate inside/outside the tolerance
1363 if ( f1 >= kp->dist_play_radius ) {
1364 //----- Apply 100% tracking if outside tolerance
1365 f1 = 1.0f ;
1366 } else {
1367 //----- If inside tolerance, weaken tracking as target gets closer
1368 f1 /= kp->dist_play_radius ;
1369 f1 *= f1 ; // Second power
1370 f1 *= f1 ; // Fourth power
1371 }
1372 f1 *= kp->dist_sensitivity ;
1373
1374 //----- Tracking
1375 sp->dist_vec = f1 * f2 ;
1376 if ( sp->dist_vec < 0.0f ) {
1377 sp->dist_speed = -sp->dist_vec ;
1378 } else {
1379 sp->dist_speed = sp->dist_vec ;
1380 }
1381
1382 sp->dist += sp->dist_vec ;
1383 } else {
1384 if ( f1 > kp->dist_play_radius ) {
1385 //----- Interpolate because outside tolerance
1386 f1 = ( f1 - kp->dist_play_radius ) / f1 * kp->dist_sensitivity ;
1387 sp->dist_vec = f1 * f2 ;
1388 if ( sp->dist_vec < 0.0f ) {
1389 sp->dist_speed = -sp->dist_vec ;
1390 } else {
1391 sp->dist_speed = sp->dist_vec ;
1392 }
1393
1394 sp->dist += sp->dist_vec ;
1395 } else {
1396 //----- Does not move since it's inside tolerance
1397 sp->dist_vec = 0.0f ;
1398 sp->dist_speed = 0.0f ;
1399 }
1400 }
1401 }
1402
1403 /***********************************************************************
1404 Calculate the pointing location
1405 ***********************************************************************/
1406 //----- Center coordinate of two marks
1407 pos.x = ( kp->kobj_regular[ 0 ].center.x + kp->kobj_regular[ 1 ].center.x ) * 0.5f ;
1408 pos.y = ( kp->kobj_regular[ 0 ].center.y + kp->kobj_regular[ 1 ].center.y ) * 0.5f ;
1409
1410 //----- Rotate for the amount of twist
1411 f1 = kp->sec_nrm.x * kp->sec_nrm_hori.x + kp->sec_nrm.y * kp->sec_nrm_hori.y ;
1412 f2 = -kp->sec_nrm.y * kp->sec_nrm_hori.x + kp->sec_nrm.x * kp->sec_nrm_hori.y ;
1413 vec.x = f1 * pos.x - f2 * pos.y ;
1414 vec.y = f2 * pos.x + f1 * pos.y ;
1415
1416 //----- Apply scaling after correcting the center position
1417 vec.x = ( kp->center_org.x - vec.x ) * kp->dpd2pos_scale ;
1418 vec.y = ( kp->center_org.y - vec.y ) * kp->dpd2pos_scale ;
1419
1420 //----- Convert to the gravitational direction coordinate system at calibration
1421 pos.x = -kp->accXY_nrm_hori.y * vec.x + kp->accXY_nrm_hori.x * vec.y ;
1422 pos.y = -kp->accXY_nrm_hori.x * vec.x - kp->accXY_nrm_hori.y * vec.y ;
1423
1424 //----- Consider the tracking and tolerance for the target value
1425 if ( sp->dpd_valid_fg == 0 ) {
1426 //----- Because this is the first pointing, initialize with the given value
1427 sp->pos = pos ;
1428 sp->vec = Vec2_0 ;
1429 sp->speed = 0.0f ;
1430 } else {
1431 //----- Difference to the target
1432 vec.x = pos.x - sp->pos.x ;
1433 vec.y = pos.y - sp->pos.y ;
1434 f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ;
1435
1436 if ( kp->pos_play_mode == KPAD_PLAY_MODE_LOOSE ) {
1437 //----- Tracking rate inside/outside the tolerance
1438 if ( f1 >= kp->pos_play_radius ) {
1439 //----- Apply 100% tracking if outside tolerance
1440 f1 = 1.0f ;
1441 } else {
1442 //----- If inside tolerance, weaken tracking as target gets closer
1443 f1 /= kp->pos_play_radius ;
1444 f1 *= f1 ; // Second power
1445 f1 *= f1 ; // Fourth power
1446 }
1447 f1 *= kp->pos_sensitivity ;
1448
1449 //----- Tracking
1450 sp->vec.x = f1 * vec.x ;
1451 sp->vec.y = f1 * vec.y ;
1452 sp->speed = sqrtf( sp->vec.x * sp->vec.x + sp->vec.y * sp->vec.y ) ;
1453
1454 sp->pos.x += sp->vec.x ;
1455 sp->pos.y += sp->vec.y ;
1456 } else {
1457 if ( f1 > kp->pos_play_radius ) {
1458 //----- Interpolate because outside tolerance
1459 f1 = ( f1 - kp->pos_play_radius ) / f1 * kp->pos_sensitivity ;
1460 sp->vec.x = f1 * vec.x ;
1461 sp->vec.y = f1 * vec.y ;
1462 sp->speed = sqrtf( sp->vec.x * sp->vec.x + sp->vec.y * sp->vec.y ) ;
1463
1464 sp->pos.x += sp->vec.x ;
1465 sp->pos.y += sp->vec.y ;
1466 } else {
1467 //----- Does not move since it's inside tolerance
1468 sp->vec = Vec2_0 ;
1469 sp->speed = 0.0f ;
1470 }
1471 }
1472 }
1473
1474 /***********************************************************************
1475 Update flags
1476 ***********************************************************************/
1477 sp->dpd_valid_fg = valid_fg_next ;
1478 }
1479
1480
1481 /*******************************************************************************
1482 KPAD DPD information load process
1483 *******************************************************************************/
read_kpad_dpd(KPADInsideStatus * kp,KPADUnifiedWpadStatus * uwp)1484 static void read_kpad_dpd( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp )
1485 {
1486 KPADStatus *sp = &kp->status ;
1487 KPADObject *op1 ;
1488 s8 valid_fg_next ;
1489
1490 /***********************************************************************
1491 Change the WPAD object to KPAD
1492 ***********************************************************************/
1493 if ( uwp->fmt == WPAD_FMT_CORE_ACC_DPD ||
1494 uwp->fmt == WPAD_FMT_FREESTYLE_ACC_DPD ||
1495 uwp->fmt == WPAD_FMT_CLASSIC_ACC_DPD ) {
1496 //----- Change the WPAD object to KPAD
1497 get_kobj( kp, &uwp->u.core.obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ;
1498 } else {
1499 // dpd data is not prepared
1500 op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
1501 do {
1502 op1->error_fg = -1 ;
1503 } while ( --op1 >= kp->kobj_sample ) ;
1504 }
1505
1506 /***********************************************************************
1507 Select the normal object
1508 ***********************************************************************/
1509 //----- Remove invalid objects
1510 check_kobj_outside_frame( kp, kp->kobj_sample ) ; // Set surrounding objects to invalid
1511 check_kobj_same_position( kp->kobj_sample ) ; // Set objects at the same coordinate to be invalid
1512
1513 //----- Determine the number shown
1514 kp->valid_objs = 0 ;
1515 op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ;
1516 do {
1517 if ( op1->error_fg == 0 ) ++ kp->valid_objs ;
1518 } while ( --op1 >= kp->kobj_sample ) ;
1519
1520 //----- Recognition process
1521 if ( sp->acc_vertical.x <= kp_err_up_inpr ) goto LABEL_select_NG ;
1522
1523 if ( sp->dpd_valid_fg == 2 || sp->dpd_valid_fg == -2 ) {
1524 //----- Recognized using two objects the last time
1525 if ( kp->valid_objs >= 2 )
1526 {
1527 valid_fg_next = select_2obj_continue( kp ) ;
1528 if ( valid_fg_next ) goto LABEL_select_OK ;
1529 }
1530 if ( kp->valid_objs >= 1 )
1531 {
1532 valid_fg_next = select_1obj_continue( kp ) ;
1533 if ( valid_fg_next ) goto LABEL_select_OK ;
1534 }
1535 } else if ( sp->dpd_valid_fg == 1 || sp->dpd_valid_fg == -1 ) {
1536 //----- Recognized using one object the last time
1537 if ( kp->valid_objs >= 2 )
1538 {
1539 valid_fg_next = select_2obj_first( kp ) ;
1540 if ( valid_fg_next ) goto LABEL_select_OK ;
1541 }
1542 if ( kp->valid_objs >= 1 )
1543 {
1544 valid_fg_next = select_1obj_continue( kp ) ;
1545 if ( valid_fg_next ) goto LABEL_select_OK ;
1546 }
1547 } else {
1548 //----- Not recognized the last time
1549 if ( kp->valid_objs >= 2 )
1550 {
1551 valid_fg_next = select_2obj_first( kp ) ;
1552
1553 if ( valid_fg_next ) goto LABEL_select_OK ;
1554 }
1555 if ( kp->valid_objs == 1 )
1556 {
1557 valid_fg_next = select_1obj_first( kp ) ;
1558 if ( valid_fg_next ) goto LABEL_select_OK ;
1559 }
1560 }
1561
1562 LABEL_select_NG :
1563
1564 valid_fg_next = 0 ; // Not selected
1565
1566 LABEL_select_OK : // Maybe selected
1567
1568 //----- Update section information if successfully selected
1569 if ( valid_fg_next ) {
1570 //----- Calculate the information of two points and object tilt.
1571 calc_obj_horizon( kp ) ;
1572
1573 //----- Error if obviously different from the acceleration tilt
1574 if ( kp->ah_circle_ct == 0 ) {
1575 if ( kp->obj_horizon.x * kp->acc_horizon.x + kp->obj_horizon.y * kp->acc_horizon.y <= kp_err_acc_inpr ) {
1576 valid_fg_next = 0 ; // Invalid after all
1577
1578 kp->kobj_regular[ 0 ].error_fg =
1579 kp->kobj_regular[ 1 ].error_fg = 1 ;
1580 }
1581 }
1582
1583 //----- Consecutive two point recognition count
1584 if ( sp->dpd_valid_fg == 2 && valid_fg_next == 2 ) {
1585 if ( kp->dpd_valid2_ct == 200 ) {
1586 kp->trust_sec_length = kp->sec_length ;
1587 } else {
1588 ++ kp->dpd_valid2_ct ;
1589 }
1590 } else {
1591 kp->dpd_valid2_ct = 0 ;
1592 }
1593 } else {
1594 kp->dpd_valid2_ct = 0 ;
1595 }
1596
1597 //----- Update application variables
1598 calc_dpd_variable( kp, valid_fg_next ) ;
1599 }
1600
1601
1602 /*******************************************************************************
1603 Clamp processing of analog trigger
1604 *******************************************************************************/
clamp_trigger(f32 * trigger,s32 tr,s32 min,s32 max)1605 static void clamp_trigger( f32 *trigger, s32 tr, s32 min, s32 max )
1606 {
1607 if ( tr <= min ) {
1608 *trigger = 0.0f ;
1609 } else if ( tr >= max ) {
1610 *trigger = 1.0f ;
1611 } else {
1612 *trigger = (f32)( tr - min ) / (f32)( max - min ) ;
1613 }
1614 }
1615
1616
1617 /*******************************************************************************
1618 Clamp processing of stick
1619 *******************************************************************************/
clamp_stick_circle(Vec2 * stick,s32 sx,s32 sy,s32 min,s32 max)1620 static void clamp_stick_circle( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max )
1621 {
1622 f32 length ;
1623 f32 fx = (f32)sx ;
1624 f32 fy = (f32)sy ;
1625 f32 fmin = (f32)min ;
1626 f32 fmax = (f32)max ;
1627
1628
1629 length = sqrtf( fx * fx + fy * fy ) ;
1630
1631 if ( length <= fmin ) {
1632 stick->x = stick->y = 0.0f ;
1633
1634 } else if ( length >= fmax ) {
1635 stick->x = fx / length ;
1636 stick->y = fy / length ;
1637
1638 } else {
1639 length = ( length - fmin ) / ( fmax - fmin ) / length ;
1640 stick->x = fx * length ;
1641 stick->y = fy * length ;
1642 }
1643 }
1644
clamp_stick_cross(Vec2 * stick,s32 sx,s32 sy,s32 min,s32 max)1645 static void clamp_stick_cross( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max )
1646 {
1647 f32 length ;
1648
1649
1650 //----- Process clamp for each axis separately
1651 if ( sx < 0 ) {
1652 clamp_trigger( &stick->x, -sx, min, max ) ;
1653 stick->x = -stick->x ;
1654 } else {
1655 clamp_trigger( &stick->x, sx, min, max ) ;
1656 }
1657 if ( sy < 0 ) {
1658 clamp_trigger( &stick->y, -sy, min, max ) ;
1659 stick->y = -stick->y ;
1660 } else {
1661 clamp_trigger( &stick->y, sy, min, max ) ;
1662 }
1663
1664 //----- Maximum length is set to 1
1665 length = stick->x * stick->x + stick->y * stick->y ;
1666 if ( length > 1.0f ) {
1667 length = sqrtf( length ) ;
1668 stick->x /= length ;
1669 stick->y /= length ;
1670 }
1671 }
1672
1673 /*******************************************************************************
1674 Load stick information
1675 *******************************************************************************/
read_kpad_stick(KPADInsideStatus * kp,KPADUnifiedWpadStatus * uwp)1676 static void read_kpad_stick( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp )
1677 {
1678 KPADEXStatus *ep = &kp->status.ex_status ;
1679 void (*clampStickFuncp)( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max ) ;
1680
1681 clampStickFuncp = kp_stick_clamp_cross ? clamp_stick_circle : clamp_stick_cross ;
1682
1683 if ( uwp->u.fs.dev == WPAD_DEV_FREESTYLE &&
1684 (uwp->fmt == WPAD_FMT_FREESTYLE ||
1685 uwp->fmt == WPAD_FMT_FREESTYLE_ACC ||
1686 uwp->fmt == WPAD_FMT_FREESTYLE_ACC_DPD) ) {
1687
1688 if ( kp->exResetReq ) {
1689 kp->exResetReq = FALSE ;
1690
1691 //----- Nunchuk unit
1692 ep->fs.stick = Vec2_0 ;
1693 ep->fs.acc.x = ep->fs.acc.z = 0.0f ;
1694 ep->fs.acc.y = -1.0f ;
1695 ep->fs.acc_value = 1.0f ;
1696 ep->fs.acc_speed = 0.0f ;
1697 }
1698
1699 //----- Stick data processing
1700 clampStickFuncp( &ep->fs.stick, uwp->u.fs.fsStickX, uwp->u.fs.fsStickY, kp_fs_fstick_min, kp_fs_fstick_max ) ;
1701 } else if ( uwp->u.cl.dev == WPAD_DEV_CLASSIC &&
1702 (uwp->fmt == WPAD_FMT_CLASSIC ||
1703 uwp->fmt == WPAD_FMT_CLASSIC_ACC ||
1704 uwp->fmt == WPAD_FMT_CLASSIC_ACC_DPD) ) {
1705
1706 if ( kp->exResetReq ) {
1707 kp->exResetReq = FALSE ;
1708
1709 //----- Classic Controller unit
1710 ep->cl.lstick = Vec2_0 ;
1711 ep->cl.rstick = Vec2_0 ;
1712 ep->cl.ltrigger = ep->cl.rtrigger = 0.0f ;
1713 ep->cl.hold = ep->cl.trig = ep->cl.release = 0x00000000 ;
1714 kp->btn_cl_repeat_time = 0 ;
1715 kp->btn_cl_repeat_next = kp->btn_repeat_delay ;
1716 }
1717
1718 //----- Stick data processing
1719 clampStickFuncp( &ep->cl.lstick, uwp->u.cl.clLStickX, uwp->u.cl.clLStickY, kp_cl_stick_min, kp_cl_stick_max ) ;
1720 clampStickFuncp( &ep->cl.rstick, uwp->u.cl.clRStickX, uwp->u.cl.clRStickY, kp_cl_stick_min, kp_cl_stick_max ) ;
1721 clamp_trigger( &ep->cl.ltrigger, uwp->u.cl.clTriggerL, kp_cl_trigger_min, kp_cl_trigger_max ) ;
1722 clamp_trigger( &ep->cl.rtrigger, uwp->u.cl.clTriggerR, kp_cl_trigger_min, kp_cl_trigger_max ) ;
1723 }
1724 }
1725
1726
1727 /*******************************************************************************
1728 READ KPAD (return the number of loaded buffer)
1729 *******************************************************************************/
KPADRead(s32 chan,KPADStatus samplingBufs[],u32 length)1730 s32 KPADRead( s32 chan, KPADStatus samplingBufs[], u32 length )
1731 {
1732 // This assumption is confirmed in KPADInit()
1733 // (sizeof(KPADTmpStatus) == sizeof(KPADStatus))
1734 KPADTmpStatus *tp = (KPADTmpStatus *)samplingBufs ;
1735 KPADUnifiedWpadStatus uwStatus ;
1736 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
1737 KPADUnifiedWpadStatus *uwp ;
1738 s32 wpad_err ;
1739 s32 idx ;
1740 u32 copy_ct ;
1741 u32 return_ct = 0 ;
1742 u32 bufCount ;
1743 BOOL enabled ;
1744 u32 lastCoreButton ;
1745 u32 lastFsButton ;
1746 u32 lastClButton ;
1747 u32 lastDev ;
1748
1749 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
1750
1751 if ( WPADGetStatus() != WPAD_STATE_SETUP ) {
1752 return ( 0 ) ;
1753 }
1754
1755 enabled = OSDisableInterrupts() ;
1756 if ( kp->readLocked ) {
1757 (void)OSRestoreInterrupts( enabled ) ;
1758 return ( 0 ) ;
1759 }
1760 kp->readLocked = TRUE ;
1761
1762 wpad_err = WPADProbe( chan, NULL ) ;
1763 if ( wpad_err == WPAD_ERR_NO_CONTROLLER &&
1764 kp->dpd_ctrl_callback &&
1765 kp->dpdPreCallbackDone &&
1766 !kp->dpdPostCallbackDone ) {
1767 KPADiControlDpdCallback( chan, WPAD_ERR_NONE ) ;
1768 }
1769
1770 (void)OSRestoreInterrupts( enabled ) ;
1771
1772 if ( kp->resetReq ) {
1773 kp->status.wpad_err = (s8)wpad_err ;
1774 reset_kpad( kp ) ;
1775 }
1776
1777
1778 // Sampling callback may have been unregistered
1779 // upon new connection
1780 WPADSetSamplingCallback( chan, KPADiSamplingCallback ) ;
1781
1782 if ( kp->bufCount &&
1783 samplingBufs &&
1784 length ) {
1785 enabled = OSDisableInterrupts() ;
1786
1787 bufCount = kp->bufCount ;
1788 //----- Check the number of times to save to the buffer
1789 if ( bufCount > length ) {
1790 copy_ct = length ;
1791 } else {
1792 copy_ct = bufCount ;
1793 }
1794 kp->bufCount = 0 ;
1795 return_ct = copy_ct ;
1796
1797 tp += copy_ct ; // Start saving old items, beginning at the end
1798
1799 idx = (s32)( kp->bufIdx - copy_ct ) ;
1800 if ( idx < 0 ) {
1801 idx += KPAD_RING_BUFS ;
1802 }
1803
1804 --tp ;
1805 while ( --copy_ct ) {
1806 --tp ;
1807 tp->w = kp->uniRingBuf[ idx ] ;
1808 idx++ ;
1809 if ( idx >= KPAD_RING_BUFS ) {
1810 idx = 0 ;
1811 }
1812 }
1813 uwStatus = kp->uniRingBuf[ idx ] ;
1814
1815 (void)OSRestoreInterrupts( enabled ) ;
1816
1817 // Obtain 1G value from the controller to absorb the individual unit differences.
1818
1819 // Although we'd like to get the value in one try, the value cannot be retrieved immediately after connect or attach.
1820 // For the time being, it is obtained here each time through the loop
1821 {
1822 WPADAcc core1G = {1, 1, 1} ;
1823 WPADAcc fs1G = {1, 1, 1} ;
1824
1825 WPADGetAccGravityUnit( chan, WPAD_DEV_CORE, &core1G ) ;
1826 if(core1G.x * core1G.y * core1G.z != 0){ // Bug fix
1827 kp->acc_scale_x = 1.0f / core1G.x ;
1828 kp->acc_scale_y = 1.0f / core1G.y ;
1829 kp->acc_scale_z = 1.0f / core1G.z ;
1830 } else {
1831 // The values listed here should not be too far off from the precise value obtained through WPADGetAccGravityUnit().
1832 //
1833 kp->acc_scale_x = 1.0f / 100 ;
1834 kp->acc_scale_y = 1.0f / 100 ;
1835 kp->acc_scale_z = 1.0f / 100 ;
1836 }
1837
1838 WPADGetAccGravityUnit( chan, WPAD_DEV_FREESTYLE, &fs1G ) ;
1839 if( fs1G.x * fs1G.y * fs1G.z != 0 ){ // Bug fix
1840 kp->fs_acc_scale_x = 1.0f / fs1G.x ;
1841 kp->fs_acc_scale_y = 1.0f / fs1G.y ;
1842 kp->fs_acc_scale_z = 1.0f / fs1G.z ;
1843 } else {
1844 // The values listed here should not be too far off from the precise value obtained through WPADGetAccGravityUnit().
1845 //
1846 kp->fs_acc_scale_x = 1.0f / 200 ;
1847 kp->fs_acc_scale_y = 1.0f / 200 ;
1848 kp->fs_acc_scale_z = 1.0f / 200 ;
1849 }
1850 }
1851
1852 // find last button data and device type
1853 copy_ct = return_ct ;
1854 lastCoreButton = lastFsButton = lastClButton = KPAD_BUTTON_MASK ;
1855 lastDev = WPAD_DEV_NOT_FOUND ;
1856 tp = (KPADTmpStatus *)samplingBufs + copy_ct ;
1857 --tp ;
1858 do {
1859 --tp ;
1860 uwp = ( copy_ct > 1 ) ? &tp->w : &uwStatus ;
1861 switch ( uwp->u.core.err ) {
1862 case WPAD_ERR_NONE :
1863 lastDev = uwp->u.core.dev ;
1864 if ( lastDev == WPAD_DEV_FREESTYLE ) {
1865 lastFsButton = uwp->u.fs.button ;
1866 lastClButton = 0 ;
1867 } else if ( lastDev == WPAD_DEV_CLASSIC ) {
1868 lastFsButton = 0 ;
1869 lastClButton = uwp->u.cl.clButton ;
1870 } else {
1871 lastFsButton = lastClButton = 0 ;
1872 }
1873 // thru
1874 case WPAD_ERR_CORRUPTED :
1875 case WPAD_ERR_BUSY :
1876 lastCoreButton = uwp->u.core.button ;
1877 break ;
1878
1879 default :
1880 break ;
1881 }
1882 } while ( --copy_ct ) ;
1883
1884 /***************************************************************
1885 Processes to be performed for each game frame
1886 ***************************************************************/
1887 //----- Load button information
1888 if ( lastCoreButton == KPAD_BUTTON_MASK ) {
1889 do {
1890 memcpy( samplingBufs, &kp->status, sizeof(KPADStatus) ) ;
1891 samplingBufs++ ;
1892 } while ( --return_ct ) ;
1893 goto finish ;
1894 }
1895 if ( lastFsButton == KPAD_BUTTON_MASK ) {
1896 lastFsButton = kp->status.hold ;
1897 }
1898 if ( lastClButton == KPAD_BUTTON_MASK ) {
1899 lastClButton = kp->status.ex_status.cl.hold ;
1900 }
1901 read_kpad_button( kp, lastDev, bufCount, lastCoreButton, lastFsButton, lastClButton ) ;
1902
1903 copy_ct = return_ct ;
1904 tp = (KPADTmpStatus *)samplingBufs + copy_ct ;
1905 --tp ;
1906
1907 do {
1908 --tp ;
1909 uwp = ( copy_ct > 1 ) ? &tp->w : &uwStatus ;
1910
1911 //----- Copy error code
1912 kp->status.wpad_err = uwp->u.core.err ;
1913 if ( kp->status.dev_type != uwp->u.core.dev ) {
1914 if ( uwp->u.core.err == WPAD_ERR_NONE ||
1915 uwp->u.core.err == WPAD_ERR_BUSY ||
1916 uwp->u.core.err == WPAD_ERR_NO_CONTROLLER ) {
1917 kp->status.dev_type = uwp->u.core.dev ;
1918 kp->exResetReq = TRUE ;
1919 }
1920 }
1921 kp->status.data_format = uwp->fmt ;
1922
1923 switch ( uwp->u.core.err ) {
1924 case WPAD_ERR_NONE :
1925 read_kpad_stick( kp, uwp ) ;
1926 // thru
1927 case WPAD_ERR_CORRUPTED :
1928 read_kpad_acc( kp, uwp ) ;
1929 read_kpad_dpd( kp, uwp ) ;
1930 break ;
1931
1932 default :
1933 kp->status.dpd_valid_fg = FALSE ;
1934 break ;
1935 }
1936 tp[ 1 ].k = kp->status ;
1937
1938 } while ( --copy_ct ) ;
1939 }
1940
1941 finish :
1942 kp->readLocked = FALSE ;
1943
1944 return (s32)return_ct ;
1945
1946 }
1947
1948
1949 /*******************************************************************************
1950 INIT KPAD
1951 *******************************************************************************/
KPADInit(void)1952 void KPADInit( void )
1953 {
1954 s32 i ;
1955 KPADInsideStatus *kp ;
1956 GXColor black = {0, 0, 0, 0} ;
1957 GXColor white = {255, 255, 255, 255} ;
1958 u32 idx ;
1959
1960 // SDK 2.0 or later is required (see SDK release note 41)
1961 // Also KPADRead() assumes: (sizeof(KPADTmpStatus) == sizeof(KPADStatus))
1962 if (offsetof( WPADFSStatus, err ) == offsetof( WPADStatus, err ) &&
1963 sizeof(KPADTmpStatus) == sizeof(KPADStatus) ) {
1964 // OK
1965 } else {
1966 OSFatal( black, white, "KPADInit error" ) ;
1967 // Never reach here.
1968 }
1969
1970 //----- WPAD
1971 WPADInit() ;
1972
1973 //----- KPAD
1974 memset( &inside_kpads, 0, sizeof(inside_kpads) ) ;
1975 kp_err_dist_max = (f32)( 1.0f + (f32)WPADGetDpdSensitivity() ) ;
1976
1977 i = 0 ;
1978 do {
1979 kp = &inside_kpads[ i ] ;
1980
1981 //----- The DPD is enabled by default
1982 kp->dpdEnabled = TRUE ;
1983 kp->dpdCmd = WPAD_DPD_OFF ;
1984
1985 //----- Check device
1986 kp->status.dev_type = WPAD_DEV_NOT_FOUND ;
1987 kp->status.data_format = WPAD_FMT_CORE ;
1988
1989 kp->dist_org = idist_org ;
1990 kp->accXY_nrm_hori = iaccXY_nrm_hori ;
1991 kp->sec_nrm_hori = isec_nrm_hori ;
1992 kp->center_org = icenter_org ;
1993 calc_dpd2pos_scale( kp ) ;
1994
1995 //----- Initialization of KPAD pointing status
1996 kp->pos_play_radius =
1997 kp->hori_play_radius =
1998 kp->dist_play_radius =
1999 kp->acc_play_radius = 0.0f ;
2000
2001 kp->pos_sensitivity =
2002 kp->hori_sensitivity =
2003 kp->dist_sensitivity =
2004 kp->acc_sensitivity = 1.0f ;
2005
2006 kp->pos_play_mode =
2007 kp->hori_play_mode =
2008 kp->dist_play_mode =
2009 kp->acc_play_mode = KPAD_PLAY_MODE_LOOSE ;
2010
2011 //----- Initialize to no button repeat
2012 KPADSetBtnRepeat( i, 0.0f, 0.0f ) ;
2013
2014 //----- Enabling error correction for the Sensor Bar placement position
2015 KPADEnableAimingMode( i ) ;
2016
2017 //----- Acceleration correction for the Nunchuk is disabled by default
2018 kp->fsAccRevise = 0 ;
2019
2020 //----- Initialize the rotation matrix
2021 MTXRowCol( kp_fs_rot, 0, 0 ) = 1 ;
2022 MTXRowCol( kp_fs_rot, 0, 1 ) = 0 ;
2023 MTXRowCol( kp_fs_rot, 0, 2 ) = 0 ;
2024 MTXRowCol( kp_fs_rot, 0, 3 ) = 0 ;
2025 MTXRowCol( kp_fs_rot, 1, 0 ) = 0 ;
2026 MTXRowCol( kp_fs_rot, 1, 1 ) = (f32) cos( MTXDegToRad(kp_fs_revise_deg) ) ;
2027 MTXRowCol( kp_fs_rot, 1, 2 ) = (f32)-sin( MTXDegToRad(kp_fs_revise_deg) ) ;
2028 MTXRowCol( kp_fs_rot, 1, 3 ) = 0 ;
2029 MTXRowCol( kp_fs_rot, 2, 0 ) = 0 ;
2030 MTXRowCol( kp_fs_rot, 2, 1 ) = (f32) sin( MTXDegToRad(kp_fs_revise_deg) ) ;
2031 MTXRowCol( kp_fs_rot, 2, 2 ) = (f32) cos( MTXDegToRad(kp_fs_revise_deg) ) ;
2032 MTXRowCol( kp_fs_rot, 2, 3 ) = 0 ;
2033
2034 idx = 0 ;
2035 do {
2036 kp->uniRingBuf[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ;
2037 } while ( ++idx < KPAD_RING_BUFS ) ;
2038
2039 } while ( ++i < WPAD_MAX_CONTROLLERS ) ;
2040
2041 //---- Initialize value
2042 KPADReset() ;
2043 OSRegisterVersion( __KPADVersion ) ;
2044 }
2045
2046
2047 /*******************************************************************************
2048 RESET KPAD
2049 *******************************************************************************/
KPADReset(void)2050 void KPADReset( void )
2051 {
2052 s32 chan ;
2053
2054 //----- Recalculate constant
2055 KPADSetObjInterval( kp_obj_interval ) ;
2056
2057 //---- Reset all KPADs
2058 chan = WPAD_MAX_CONTROLLERS - 1 ;
2059 do {
2060 if ( WPADGetStatus() == WPAD_STATE_SETUP ) {
2061 WPADStopMotor( chan ) ;
2062 }
2063 inside_kpads[ chan ].resetReq = TRUE ;
2064 } while ( --chan >= 0 ) ;
2065 }
2066
KPADDisableDPD(const s32 chan)2067 void KPADDisableDPD( const s32 chan )
2068 {
2069 inside_kpads[ chan ].dpdEnabled = FALSE ;
2070 }
2071
KPADEnableDPD(const s32 chan)2072 void KPADEnableDPD ( const s32 chan )
2073 {
2074 inside_kpads[ chan ].dpdEnabled = TRUE ;
2075 }
2076
KPADSetControlDpdCallback(s32 chan,KPADControlDpdCallback callback)2077 void KPADSetControlDpdCallback( s32 chan, KPADControlDpdCallback callback )
2078 {
2079 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2080 BOOL enabled ;
2081
2082 enabled = OSDisableInterrupts() ;
2083 kp->dpd_ctrl_callback = callback ;
2084 (void)OSRestoreInterrupts( enabled ) ;
2085 }
2086
KPADiSamplingCallback(s32 chan)2087 static void KPADiSamplingCallback(s32 chan)
2088 {
2089 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2090 KPADUnifiedWpadStatus *uwp ;
2091 u32 idx ;
2092 u32 type ;
2093 u32 curDpd ;
2094 f32 aimClbr ;
2095 static struct {
2096 u8 dpd ;
2097 u8 fmt ;
2098 } table[] = {
2099 { WPAD_DPD_OFF, WPAD_FMT_CORE_ACC },
2100 { WPAD_DPD_EXP, WPAD_FMT_CORE_ACC_DPD },
2101 { WPAD_DPD_OFF, WPAD_FMT_FREESTYLE_ACC },
2102 { WPAD_DPD_STD, WPAD_FMT_FREESTYLE_ACC_DPD },
2103 { WPAD_DPD_OFF, WPAD_FMT_CLASSIC_ACC },
2104 { WPAD_DPD_STD, WPAD_FMT_CLASSIC_ACC_DPD }
2105 } ;
2106
2107 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) );
2108
2109 if ( WPADProbe( chan, &type ) == WPAD_ERR_NO_CONTROLLER ) {
2110 goto finish ;
2111 }
2112
2113 idx = kp->bufIdx ;
2114 if ( idx >= KPAD_RING_BUFS ) {
2115 idx = 0 ;
2116 }
2117
2118 uwp = &kp->uniRingBuf[ idx ] ;
2119 WPADRead( chan, &uwp->u ) ;
2120 uwp->fmt = (u8)WPADGetDataFormat( chan ) ;
2121
2122 kp->bufIdx = (u8)( idx + 1 ) ;
2123 if ( kp->bufCount < KPAD_RING_BUFS ) {
2124 kp->bufCount++ ;
2125 }
2126
2127 if ( kp->aimReq ) {
2128 if ( kp->aimEnabled ) {
2129 if ( WPAD_SENSOR_BAR_POS_TOP == WPADGetSensorBarPosition() ) {
2130 aimClbr = 0.2f ;
2131 } else {
2132 aimClbr = -0.2f ;
2133 }
2134 } else {
2135 aimClbr = 0.0f ;
2136 }
2137 KPADSetSensorHeight( chan, aimClbr ) ;
2138
2139 kp->aimReq = FALSE ;
2140 }
2141
2142 //----- Check DPD settings
2143 switch ( type ) {
2144 case WPAD_DEV_CORE :
2145 case WPAD_DEV_FUTURE :
2146 case WPAD_DEV_NOT_SUPPORTED :
2147 case WPAD_DEV_UNKNOWN :
2148 idx = 0 ;
2149 break ;
2150
2151 case WPAD_DEV_FREESTYLE :
2152 idx = 2 ;
2153 break ;
2154
2155 case WPAD_DEV_CLASSIC :
2156 idx = 4 ;
2157 break ;
2158
2159 default :
2160 goto finish ;
2161 }
2162
2163 if ( kp->dpdEnabled ) {
2164 idx += 1 ;
2165 }
2166
2167 curDpd = (u32)( WPADIsDpdEnabled( chan ) ? kp->dpdCmd : WPAD_DPD_OFF );
2168
2169 if ( curDpd != table[idx].dpd ) {
2170 if ( kp->dpd_ctrl_callback &&
2171 !kp->dpdPreCallbackDone ) {
2172 kp->dpdPreCallbackDone = TRUE ;
2173 kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_START ) ;
2174 kp->dpdPostCallbackDone = FALSE ;
2175 }
2176 if ( !kp->dpdIssued ) {
2177 kp->dpdIssued = TRUE ;
2178 if (WPADControlDpd( chan, table[ idx ].dpd, KPADiControlDpdCallback )
2179 == WPAD_ERR_NONE ) {
2180 kp->dpdCmd = table[ idx ].dpd ;
2181 }
2182 }
2183 } else {
2184 if ( uwp->fmt != table[ idx ].fmt ) {
2185 WPADSetDataFormat( chan, table[ idx ].fmt );
2186 }
2187 }
2188
2189 finish :
2190 if ( kp->appSamplingCallback ) {
2191 kp->appSamplingCallback( chan ) ;
2192 }
2193 }
2194
KPADiControlDpdCallback(s32 chan,s32 result)2195 static void KPADiControlDpdCallback( s32 chan, s32 result )
2196 {
2197 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2198
2199 if ( result == WPAD_ERR_NONE ) {
2200 if ( kp->dpd_ctrl_callback &&
2201 !kp->dpdPostCallbackDone ) {
2202 kp->dpdPostCallbackDone = TRUE ;
2203 kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_FINISHED ) ;
2204 kp->dpdPreCallbackDone = FALSE ;
2205 }
2206 }
2207 kp->dpdIssued = FALSE ;
2208 }
2209
KPADGetUnifiedWpadStatus(s32 chan,KPADUnifiedWpadStatus * dst,u32 count)2210 void KPADGetUnifiedWpadStatus( s32 chan, KPADUnifiedWpadStatus *dst, u32 count )
2211 {
2212 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2213 BOOL enabled ;
2214 u32 idx ;
2215
2216 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2217
2218 if ( count > KPAD_RING_BUFS ) {
2219 count = KPAD_RING_BUFS ;
2220 }
2221
2222 enabled = OSDisableInterrupts() ;
2223
2224 idx = kp->bufIdx ;
2225 while ( count-- ) {
2226 if ( idx == 0 ) {
2227 idx = KPAD_RING_BUFS - 1 ;
2228 } else {
2229 idx-- ;
2230 }
2231 // <== latest ... oldest ==>
2232 // dst[0] dst[1] ... dst[KPAD_RING_BUFS - 1]
2233 if ( WPADGetStatus() != WPAD_STATE_SETUP ) {
2234 kp->uniRingBuf[ idx ].u.core.err = WPAD_ERR_INVALID ;
2235 }
2236 memcpy( dst, &kp->uniRingBuf[idx], sizeof(KPADUnifiedWpadStatus) ) ;
2237 dst++ ;
2238 }
2239
2240 (void)OSRestoreInterrupts( enabled ) ;
2241 }
2242
KPADEnableStickCrossClamp(void)2243 void KPADEnableStickCrossClamp( void )
2244 {
2245 kp_stick_clamp_cross = TRUE ;
2246 }
2247
KPADDisableStickCrossClamp(void)2248 void KPADDisableStickCrossClamp( void )
2249 {
2250 kp_stick_clamp_cross = FALSE ;
2251 }
2252
KPADSetSamplingCallback(s32 chan,WPADSamplingCallback callback)2253 void KPADSetSamplingCallback( s32 chan, WPADSamplingCallback callback )
2254 {
2255 inside_kpads[ chan ].appSamplingCallback = callback ;
2256 }
2257
KPADSetReviseMode(s32 chan,BOOL sw)2258 void KPADSetReviseMode( s32 chan, BOOL sw )
2259 {
2260 KPADInsideStatus *kp = &inside_kpads[ chan ] ;
2261
2262 kp->fsAccRevise = (u8)sw ;
2263 }
2264
KPADReviseAcc(Vec * acc)2265 f32 KPADReviseAcc( Vec *acc )
2266 {
2267 MTXMultVec( kp_fs_rot, acc, acc ) ;
2268
2269 return kp_fs_revise_deg ; // Return the correction angle (in degrees)
2270 }
2271
KPADGetReviseAngle(void)2272 f32 KPADGetReviseAngle( void )
2273 {
2274 return kp_fs_revise_deg ; // Return the correction angle (in degrees)
2275 }
2276
2277
2278 /*******************************************************************************
2279 Control for all calculation methods
2280 *******************************************************************************/
KPADSetPosPlayMode(s32 chan,KPADPlayMode mode)2281 void KPADSetPosPlayMode( s32 chan, KPADPlayMode mode )
2282 {
2283 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2284 inside_kpads[ chan ].pos_play_mode = mode ;
2285 }
2286
KPADSetHoriPlayMode(s32 chan,KPADPlayMode mode)2287 void KPADSetHoriPlayMode( s32 chan, KPADPlayMode mode )
2288 {
2289 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2290 inside_kpads[ chan ].hori_play_mode = mode ;
2291 }
2292
KPADSetDistPlayMode(s32 chan,KPADPlayMode mode)2293 void KPADSetDistPlayMode( s32 chan, KPADPlayMode mode )
2294 {
2295 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2296 inside_kpads[ chan ].dist_play_mode = mode ;
2297 }
2298
KPADSetAccPlayMode(s32 chan,KPADPlayMode mode)2299 void KPADSetAccPlayMode( s32 chan, KPADPlayMode mode )
2300 {
2301 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2302 inside_kpads[ chan ].acc_play_mode = mode ;
2303 }
2304
KPADGetPosPlayMode(s32 chan)2305 KPADPlayMode KPADGetPosPlayMode( s32 chan )
2306 {
2307 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2308 return ( inside_kpads[ chan ].pos_play_mode ) ;
2309 }
2310
KPADGetHoriPlayMode(s32 chan)2311 KPADPlayMode KPADGetHoriPlayMode( s32 chan )
2312 {
2313 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2314 return ( inside_kpads[ chan ].hori_play_mode ) ;
2315 }
2316
KPADGetDistPlayMode(s32 chan)2317 KPADPlayMode KPADGetDistPlayMode( s32 chan )
2318 {
2319 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2320 return ( inside_kpads[ chan ].dist_play_mode ) ;
2321 }
2322
KPADGetAccPlayMode(s32 chan)2323 KPADPlayMode KPADGetAccPlayMode( s32 chan )
2324 {
2325 ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ;
2326 return ( inside_kpads[ chan ].acc_play_mode ) ;
2327 }
2328
2329