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