/*---------------------------------------------------------------------------* Project: KPAD library version 2 File: KPAD.c Programmers: Keizo Ohta HIRATSU Daisuke Tojo Haruki Tetsuya Sasaki Copyright 2005-2008 Nintendo. All rights reserved. These coded instructions, statements, and computer programs contain proprietary information of Nintendo of America Inc. and/or Nintendo Company Ltd., and are protected by Federal copyright law. They may not be disclosed to third parties or copied or duplicated in any form, in whole or in part, without the prior written consent of Nintendo. *---------------------------------------------------------------------------*/ #include "KPADinside.h" #include #include #include #include #include REVOLUTION_LIB_VERSION(KPAD); /******************************************************* VARIABLES *******************************************************/ //----- DPD calibration initial values static Vec2 icenter_org = { 0.000f, 0.000f } ; // Center coordinate of two marks in CMOS static f32 idist_org = 1.000f ; // Distance during calibration (in meters) static Vec2 iaccXY_nrm_hori = { 0.000f,-1.000f } ; // Direction of XY acceleration when the controller is placed horizontally static Vec2 isec_nrm_hori = { 1.000f, 0.000f } ; // Direction from left mark to right mark when the controller is placed horizontally f32 kp_obj_interval = 0.200f ; // Separation between marks at each extreme (in meters) //----- Various adjustments f32 kp_acc_horizon_pw = 0.050f ; // Calculating twist from acceleration f32 kp_ah_circle_radius = 0.070f ; // Static determination radius f32 kp_ah_circle_pw = 0.060f ; // Static determination tracking level u16 kp_ah_circle_ct = 100 ; // Static determination count BOOL kp_stick_clamp_cross = FALSE ; // Perform circular clamping //----- Numerical values regarded as errors f32 kp_err_outside_frame = 0.050f ; // Width of surrounding area where center of mass coordinate is invalid (not all surrounding lights are necessarily shown) f32 kp_err_dist_min ; // Automatic calculation of minimum operational distance (in meters) f32 kp_err_dist_max = 3.000f ; // Maximum operational distance (in meters) f32 kp_err_dist_speed = 0.040f ; // Acceptable range of change in distance (in meters) f32 kp_err_first_inpr = 0.900f ; // Dot product of acceleration tilt and object tilt when selecting two points for the first time f32 kp_err_next_inpr = 0.900f ; // Acceptable range of change in tilt (internal product value) f32 kp_err_acc_inpr = 0.900f ; // Acceptable range for internal product with static acceleration tilt f32 kp_err_up_inpr = 0.700f ; // Acceptable range for internal product with controller pointed upwards f32 kp_err_near_pos = 0.100f ; // Distance from the previous point when selecting one point as a continuation //----- For internal processing //static Vec2 kobj_frame_min, kobj_frame_max ; // Range over which center of gravity coordinates are enabled //static f32 kp_err_dist_speed_1 ; // Reciprocal //static f32 kp_err_dist_speedM_1 ; // Negative reciprocal //static f32 kp_ah_circle_radius2 ; // Square static f32 kp_dist_vv1 ; // Constants static s32 kp_fs_fstick_min = 15 ; // Nunchuk unit stick clamp settings static s32 kp_fs_fstick_max = 71 ; static s32 kp_cl_stick_min = 60 ; // Classic Controller unit stick clamp settings static s32 kp_cl_stick_max = 308 ; static s32 kp_cl_trigger_min = 30 ; // Classic Controller unit analog trigger clamp settings static s32 kp_cl_trigger_max = 180 ; //static s32 kp_gc_mstick_min = 15 ; // Old GC3D stick clamp setting //static s32 kp_gc_mstick_max = 77 ; //static s32 kp_gc_cstick_min = 15 ; // Old GCC stick clamp setting //static s32 kp_gc_cstick_max = 64 ; //static s32 kp_gc_trigger_min = 30 ; // Old GC analog trigger clamp setting //static s32 kp_gc_trigger_max = 180 ; static f32 kp_rm_acc_max = 3.4f ; // Wii Remote acceleration clamp settings static f32 kp_fs_acc_max = 2.1f ; // Nunchuk acceleration clamp settings static s32 kp_ex_trigger_min = 0 ; // Clamp settings for analog trigger buttons on an extension controller unit static s32 kp_ex_trigger_max = 256 ; static s32 kp_ex_analog_min = 0 ; // Clamp settings for analog input from an extension controller unit static s32 kp_ex_analog_max = 1024 ; //----- For the Wii Balance Board static u8 kp_wbc_wait_count = 50 ; // Wait count for temperature stability on the Wii Balance Board static f32 kp_wbc_ave_count = 400 ; // Wait count for value stability on the Wii Balance Board static u8 kp_wbc_issued ; static u8 kp_wbc_enabled ; static u8 kp_wbc_setup ; static u8 kp_wbc_tgc_weight_issued ; static u8 kp_wbc_zero_point_done ; static u16 kp_wbc_ave_sample_count ; static double kp_wbc_tgc_weight ; static double kp_wbc_ave_sample[ WPAD_PRESS_UNITS ] ; static double kp_wbc_weight_ave[ WPAD_PRESS_UNITS ] ; static u8 kp_initialized = 0 ; // Has the KPADInit function been called? //----- KPAD KPADInsideStatus inside_kpads[ WPAD_MAX_CONTROLLERS ] ; //----- Zero vector static Vec2 Vec2_0 = { 0.0f, 0.0f } ; //----- Used for Nunchuk acceleration correction static Mtx kp_fs_rot ; // Rotation matrix static f32 kp_fs_revise_deg = 24.0f ; // Correction angle (in degrees) static void KPADiSamplingCallback ( s32 chan ) ; static void KPADiConnectCallback ( s32 chan, s32 reason ) ; static void KPADiControlDpdCallback ( s32 chan, s32 result ) ; static void KPADiControlWbcCallback ( s32 chan, s32 result ) ; static void KPADiUpdateTempWbcCallback( s32 chan, s32 result ) ; static s32 KPADiRead( s32 chan, KPADStatus samplingBufs[], u32 length, s32 *err, BOOL keep ) ; /******************************************************* EXTERN *******************************************************/ //----- Function to emit a warning when a callback used by the KPAD library is overwritten (this function is only used by the KPAD library) extern void WPADSetCallbackByKPAD( BOOL use ) ; // These APIs are not recommended. // Please use the KPADGetUnifiedWpadStatus function instead. // These are provided for compatibility with KPAD1. But not 100% compatible. // These functions are not supported in SDK 3.2 or later. #if 0 static void *get_ring_buffer_by_kpad1_style( s32 chan, void *buf, u32 size ) ; /******************************************************************************* Get the ring buffer of WPADStatus *******************************************************************************/ WPADStatus *KPADGetWPADRingBuffer( s32 chan ) { static WPADStatus status[ KPAD_RING_BUFS ] ; return (WPADStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_CORE ) ; } WPADFSStatus *KPADGetWPADFSRingBuffer( s32 chan ) { static WPADFSStatus status[ KPAD_RING_BUFS ] ; return (WPADFSStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_FREESTYLE ) ; } WPADCLStatus *KPADGetWPADCLRingBuffer( s32 chan ) { static WPADCLStatus status[ KPAD_RING_BUFS ] ; return (WPADCLStatus *)get_ring_buffer_by_kpad1_style( chan, &status, WPAD_DEV_CLASSIC ) ; } static void *get_ring_buffer_by_kpad1_style( s32 chan, void *buf, u32 dev ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; BOOL enabled ; s32 idx1 ; s32 idx2 ; u32 count ; u32 size ; u32 type ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; switch ( dev ) { case WPAD_DEV_CORE : size = sizeof( WPADStatus ) ; break ; case WPAD_DEV_FREESTYLE : size = sizeof( WPADFSStatus ) ; break ; case WPAD_DEV_CLASSIC : size = sizeof( WPADCLStatus ) ; break ; default : return buf ; } enabled = OSDisableInterrupts() ; idx1 = kp->bufIdx - 1 ; idx2 = (s32)WPADGetLatestIndexInBuf( chan ) ; for ( count = 0; count < KPAD_RING_BUFS; count++ ) { if ( idx1 < 0 ) { idx1 = KPAD_RING_BUFS - 1 ; } if ( idx2 < 0 ) { idx2 = KPAD_RING_BUFS - 1 ; } switch( kp->uniRingBuf[idx1].u.core.dev ) { case WPAD_DEV_CORE: case WPAD_DEV_FUTURE: case WPAD_DEV_NOT_SUPPORTED: case WPAD_DEV_UNKNOWN: type = WPAD_DEV_CORE; break; case WPAD_DEV_FREESTYLE: type = WPAD_DEV_FREESTYLE; break; case WPAD_DEV_CLASSIC: type = WPAD_DEV_CLASSIC; break; default: // Unreachable here. type = WPAD_DEV_UNKNOWN; break; } if ( type == dev ) { if ( WPADGetStatus() != WPAD_STATE_SETUP ) { kp->uniRingBuf[idx1].u.core.err = WPAD_ERR_INVALID ; } memcpy((u8 *)buf + idx2 * size, &kp->uniRingBuf[ idx1 ].u, size) ; } idx1-- ; idx2-- ; } (void)OSRestoreInterrupts( enabled ) ; return buf ; } #endif /******************************************************************************* Analog Data Clamp Settings *******************************************************************************/ void KPADSetFSStickClamp( s8 min, s8 max ) { kp_fs_fstick_min = (s32)min ; kp_fs_fstick_max = (s32)max ; } /******************************************************************************* Configure the Button Repeat Rate *******************************************************************************/ void KPADSetBtnRepeat( s32 chan, f32 delay_sec, f32 pulse_sec ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; if ( pulse_sec ) { //----- Set repeat flag setting kp->btn_repeat_delay = (u16)(s32)( delay_sec * 200.0f + 0.5f ) ; kp->btn_repeat_pulse = (u16)(s32)( pulse_sec * 200.0f + 0.5f ) ; } else { //----- No repeat flag setting kp->btn_repeat_delay = KPAD_BTN_NO_RPT_DELAY ; kp->btn_repeat_pulse = 0 ; } //----- Reset kp->btn_repeat_time = 0 ; kp->btn_repeat_next = kp->btn_repeat_delay ; kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_repeat_delay ; } /******************************************************************************* Set Marker Placement Interval (in meters) *******************************************************************************/ void KPADSetObjInterval( f32 interval ) { #pragma unused( interval ) // KPADSetObjInterval is obsolete. } static void set_obj_interval( f32 interval ) { BOOL enabled = OSDisableInterrupts() ; kp_obj_interval = interval ; //----- DPD operational minimum distance (so that the length between marks will be half that of lens diameter) kp_err_dist_min = interval / KPAD_CMOS_HFOV_TAN ; //----- Constants for calculating distance kp_dist_vv1 = interval / KPAD_CMOS_HFOV_TAN ; (void)OSRestoreInterrupts( enabled ) ; } /******************************************************************************* Parameter set *******************************************************************************/ void KPADSetPosParam( s32 chan, f32 play_radius, f32 sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].pos_play_radius = play_radius ; inside_kpads[ chan ].pos_sensitivity = sensitivity ; } void KPADSetHoriParam( s32 chan, f32 play_radius, f32 sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ); inside_kpads[ chan ].hori_play_radius = play_radius ; inside_kpads[ chan ].hori_sensitivity = sensitivity ; } void KPADSetDistParam( s32 chan, f32 play_radius, f32 sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].dist_play_radius = play_radius ; inside_kpads[ chan ].dist_sensitivity = sensitivity ; } void KPADSetAccParam( s32 chan, f32 play_radius, f32 sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].acc_play_radius = play_radius ; inside_kpads[ chan ].acc_sensitivity = sensitivity ; } /******************************************************************************* Get Parameters *******************************************************************************/ void KPADGetPosParam( s32 chan, f32 *play_radius, f32 *sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; *play_radius = inside_kpads[ chan ].pos_play_radius ; *sensitivity = inside_kpads[ chan ].pos_sensitivity ; } void KPADGetHoriParam( s32 chan, f32 *play_radius, f32 *sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; *play_radius = inside_kpads[ chan ].hori_play_radius ; *sensitivity = inside_kpads[ chan ].hori_sensitivity ; } void KPADGetDistParam( s32 chan, f32 *play_radius, f32 *sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; *play_radius = inside_kpads[ chan ].dist_play_radius ; *sensitivity = inside_kpads[ chan ].dist_sensitivity ; } void KPADGetAccParam( s32 chan, f32 *play_radius, f32 *sensitivity ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; *play_radius = inside_kpads[ chan ].acc_play_radius ; *sensitivity = inside_kpads[ chan ].acc_sensitivity ; } /******************************************************************************* Seek an easy-to-use scale value from the calibrated center position *******************************************************************************/ static void calc_dpd2pos_scale( KPADInsideStatus *kp ) { f32 scale ; f32 sx,sy ; //----- Movable distance of the controller in the vertical and horizontal directions sx = 1.0f ; // Horizontal sy = (f32)KPAD_DPD_RESO_WY / (f32)KPAD_DPD_RESO_WX ; // Vertical //----- Longest movable distance of the controller scale = sqrtf( sx * sx + sy * sy ) ;// Diagonal //----- Correct the horizontal movable distance if ( kp->center_org.x < 0.0f ) { sx += kp->center_org.x ; } else { sx -= kp->center_org.x ; } //----- Correct the vertical movable distance if ( kp->center_org.y < 0.0f ) { sy += kp->center_org.y ; } else { sy -= kp->center_org.y ; } //----- Scale that will cover the longest distance of the smaller of the range of movable distance kp->dpd2pos_scale = scale / ( ( sx < sy ) ? sx : sy ) ; } /******************************************************************************* Initialize KPAD Values *******************************************************************************/ static void reset_kpad( KPADInsideStatus *kp ) { KPADObject *op ; KPADStatus *sp = &kp->status ; KPADEXStatus *ep = &kp->status.ex_status ; kp->resetReq = FALSE ; //----- Recalculate constants kp->kobj_frame_min.x = -1.0f + kp_err_outside_frame ; kp->kobj_frame_max.x = 1.0f - kp_err_outside_frame ; kp->kobj_frame_min.y = -((f32)KPAD_DPD_RESO_WY / KPAD_DPD_RESO_WX) + kp_err_outside_frame ; kp->kobj_frame_max.y = ((f32)KPAD_DPD_RESO_WY / KPAD_DPD_RESO_WX) - kp_err_outside_frame ; kp->err_dist_speed_1 = 1.0f / kp_err_dist_speed ; kp->err_dist_speedM_1 = -1.0f / kp_err_dist_speed ; kp->ah_circle_radius2 = kp_ah_circle_radius * kp_ah_circle_radius ; kp->err_dist_min = kp_err_dist_min ; kp->dist_vv1 = kp_dist_vv1 ; //----- Clear button information sp->hold = sp->trig = sp->release = 0x00000000 ; kp->btn_repeat_time = 0 ; kp->btn_repeat_next = kp->btn_repeat_delay ; //----- Clear DPD information sp->dpd_valid_fg = 0 ; // Disabled kp->dpd_valid2_ct = 0 ; sp->pos = sp->vec = Vec2_0 ; sp->speed = 0.0f ; sp->horizon.x = kp->acc_horizon.x = kp->obj_horizon.x = 1.0f ; sp->horizon.y = kp->acc_horizon.y = kp->obj_horizon.y = 0.0f ; sp->hori_vec = Vec2_0 ; sp->hori_speed = 0.0f ; sp->acc_vertical.x = 1.0f ; sp->acc_vertical.y = 0.0f ; sp->dist = kp->dist_org ; sp->dist_vec = sp->dist_speed = 0.0f ; kp->sec_dist = sp->dist ; kp->sec_length = kp->trust_sec_length = kp->dist_vv1 / kp->sec_dist ; kp->sec_nrm = kp->sec_nrm_hori ; //----- Clear acceleration information sp->acc.x = sp->acc.z = 0.0f ; sp->acc.y = -1.0f ; sp->acc_value = 1.0f ; sp->acc_speed = 0.0f ; kp->hard_acc = sp->acc ; kp->ah_circle_pos = kp->acc_horizon ; kp->ah_circle_ct = kp_ah_circle_ct ; //----- Clear individual object information kp->valid_objs = 0 ; op = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { op->error_fg = -1 ; // Not applied } while ( --op >= kp->kobj_sample ) ; op = &kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ] ; do { op->error_fg = -1 ; // Not applied } while ( --op >= kp->kobj_regular ) ; //----- Other kp->bufCount = 0 ; // Ring buffer unprocessed //----- Clear extension controller information kp->exResetReq = TRUE ; } /******************************************************************************* Convert DPD Coordinates to Projection Coordinate System *******************************************************************************/ void KPADGetProjectionPos( Vec2 *dst, const Vec2 *src, const Rect *projRect, f32 viRatio ) { f32 projection_height = projRect->bottom - projRect->top ; // Convert the normalized values into projection coordinates. (*dst).x = src->x * (projection_height / 2.0f) * 1.2f ; (*dst).y = src->y * (projection_height / 2.0f) * 1.2f ; // Horizontal direction pixel ratio correction (*dst).x *= viRatio * 0.908 ; } /******************************************************************************* Calibration process *******************************************************************************/ s32 KPADCalibrateDPD( s32 chan ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; KPADStatus *sp = &kp->status ; KPADObject *op1, *op2 ; f32 f1, vx,vy ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; if ( kp->valid_objs != KPAD_USE_OBJECTS ) return ( kp->valid_objs ) ; /*********************************************************************** Acceleration during calibration ***********************************************************************/ vx = kp->hard_acc.x ; vy = kp->hard_acc.y ; f1 = sqrtf( vx * vx + vy * vy ) ; if ( f1 <= 0.5f ) return (-1) ; // Abnormal acceleration kp->accXY_nrm_hori.x = vx / f1 ; kp->accXY_nrm_hori.y = vy / f1 ; /*********************************************************************** Object location during calibration ***********************************************************************/ //----- Determine the mark order by location op1 = kp->kobj_sample ; while ( op1->error_fg != 0 ) ++op1 ; op2 = op1 + 1 ; while ( op2->error_fg != 0 ) ++op2 ; if ( op1->center.x < op2->center.x ) goto LABEL_cp12 ; else if ( op2->center.x < op1->center.x ) goto LABEL_cp21 ; else if ( op2->center.y < op1->center.y ) goto LABEL_cp21 ; LABEL_cp12: kp->kobj_regular[ 0 ] = *op1 ; kp->kobj_regular[ 1 ] = *op2 ; goto LABEL_cpend ; LABEL_cp21: kp->kobj_regular[ 0 ] = *op2 ; kp->kobj_regular[ 1 ] = *op1 ; LABEL_cpend: //kp->center_org.x = ( kp->kobj_regular[0].center.x + kp->kobj_regular[1].center.x ) * 0.5f ; //kp->center_org.y = ( kp->kobj_regular[0].center.y + kp->kobj_regular[1].center.y ) * 0.5f ; //kp->center_org.x = kp->center_org.y = 0.0f ; calc_dpd2pos_scale( kp ) ; //----- Section direction when the controller is in horizontal position vx = kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ].center.x - kp->kobj_regular[ 0 ].center.x ; vy = kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ].center.y - kp->kobj_regular[ 0 ].center.y ; f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not be zero kp->sec_nrm_hori.x = vx * f1 ; kp->sec_nrm_hori.y = vy * f1 ; /*********************************************************************** Distance during calibration ***********************************************************************/ kp->dist_org = kp->dist_vv1 * f1 ; /*********************************************************************** Other ***********************************************************************/ sp->dpd_valid_fg = 0 ; // Invalid for now return ( kp->valid_objs ) ; } /******************************************************************************* Enable Wii Remote Orientation Correction *******************************************************************************/ void KPADEnableAimingMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].aimReq = TRUE ; inside_kpads[ chan ].aimEnabled = TRUE ; } /******************************************************************************* Disable Wii Remote Orientation Correction *******************************************************************************/ void KPADDisableAimingMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ); inside_kpads[ chan ].aimReq = TRUE ; inside_kpads[ chan ].aimEnabled = FALSE ; } /******************************************************************************* Calibration process *******************************************************************************/ void KPADSetSensorHeight( s32 chan, f32 level ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; kp->center_org.x = 0.0f ; kp->center_org.y = -level ; calc_dpd2pos_scale( kp ) ; } /******************************************************************************* Digital Button Repeat Process *******************************************************************************/ static void calc_button_repeat( KPADInsideStatus *kp, u32 dev_type, u32 count ) { KPADStatus *sp = &kp->status ; KPADEXStatus *ep = &kp->status.ex_status ; if ( sp->trig != 0 || sp->release != 0 ) { //----- Reset because the button state changed kp->btn_repeat_time = 0 ; kp->btn_repeat_next = kp->btn_repeat_delay ; //----- Set flags also at the beginning of button press (but only when repeat is set) if ( sp->trig && kp->btn_repeat_pulse ) { sp->hold |= KPAD_BUTTON_RPT ; } } else if ( sp->hold != 0 ) { //----- Advance time because the button is pushed and its state is not changed kp->btn_repeat_time += count ; if ( kp->btn_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) { kp->btn_repeat_time -= KPAD_BTN_NO_RPT_DELAY ; } //----- Set flag when the repeat time is reached if ( kp->btn_repeat_time >= kp->btn_repeat_next ) { sp->hold |= KPAD_BUTTON_RPT ; //----- Set the next repeat time kp->btn_repeat_next += kp->btn_repeat_pulse ; //----- Loop if the time has exceeded its range here if ( kp->btn_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) { kp->btn_repeat_time -= KPAD_BTN_RPT_TIME_MAX ; kp->btn_repeat_next -= KPAD_BTN_RPT_TIME_MAX ; } } } if ( dev_type == WPAD_DEV_CLASSIC || dev_type == WPAD_DEV_GUITAR || dev_type == WPAD_DEV_TRAIN ) { if ( ep->cl.trig != 0 || ep->cl.release != 0 ) { //----- Reset because the button state changed kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_repeat_delay ; //----- Set flags also at the beginning of button press (but only when repeat is set) if ( ep->cl.trig && kp->btn_repeat_pulse ) { ep->cl.hold |= KPAD_BUTTON_RPT ; } } else if ( ep->cl.hold != 0 ) { //----- Advance time because the button is pushed and its state is not changed kp->btn_cl_repeat_time += count ; if ( kp->btn_cl_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) { kp->btn_cl_repeat_time -= KPAD_BTN_NO_RPT_DELAY ; } //----- Set flag when the repeat time is reached if ( kp->btn_cl_repeat_time >= kp->btn_cl_repeat_next ) { ep->cl.hold |= KPAD_BUTTON_RPT ; //----- Set the next repeat time kp->btn_cl_repeat_next += kp->btn_repeat_pulse ; //----- Loop if the time has exceeded its range here if ( kp->btn_cl_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) { kp->btn_cl_repeat_time -= KPAD_BTN_RPT_TIME_MAX ; kp->btn_cl_repeat_next -= KPAD_BTN_RPT_TIME_MAX ; } } } } } /******************************************************************************* KPAD Button Information Loading *******************************************************************************/ static void read_kpad_button( KPADInsideStatus *kp, u32 dev_type, u32 count, u32 core, u32 fs, u32 cl) { KPADStatus *sp = &kp->status ; KPADEXStatus *ep = &kp->status.ex_status ; u32 old_fg, change_fg ; u32 cl_old_fg, cl_change_fg ; u32 ex ; //----- Store the previous value old_fg = sp->hold & KPAD_BUTTON_MASK ; //----- Load new value sp->hold = ( core & (KPAD_BUTTON_MASK & ~(WPAD_BUTTON_Z | WPAD_BUTTON_C)) ) ; //----- Load Nunchuk buttons if ( dev_type == WPAD_DEV_FREESTYLE ) { if ( kp->exResetReq ) { ex = 0 ; } else { ex = fs ; } sp->hold |= ( ex & (WPAD_BUTTON_Z | WPAD_BUTTON_C) ) ; } //----- Button state processing change_fg = sp->hold ^ old_fg ; // Changed button sp->trig = change_fg & sp->hold ; // Pressed button sp->release = change_fg & old_fg ; // Released button //----- Load buttons for other extension controllers if ( dev_type == WPAD_DEV_CLASSIC || dev_type == WPAD_DEV_GUITAR || dev_type == WPAD_DEV_TRAIN ) { if ( kp->exResetReq ) { ex = 0 ; } else { ex = cl ; } //----- Store the previous value cl_old_fg = ep->cl.hold & KPAD_BUTTON_MASK ; //----- Load new value ep->cl.hold = ex & KPAD_BUTTON_MASK ; //----- Button state processing cl_change_fg = ep->cl.hold ^ cl_old_fg ; ep->cl.trig = cl_change_fg & ep->cl.hold ; ep->cl.release = cl_change_fg & cl_old_fg ; } //----- Repeat processing calc_button_repeat( kp, dev_type, count ) ; } /******************************************************************************* Acceleration Tracking *******************************************************************************/ static void calc_acc( KPADInsideStatus *kp, f32 *acc, f32 acc2 ) { f32 f1,f2 ; //----- Difference to the target value f2 = acc2 - *acc ; if ( kp->acc_play_mode == KPAD_PLAY_MODE_LOOSE ) { if ( f2 < 0.0f ) { f1 = -f2 ; } else { f1 = f2 ; } //----- Calculation of tracking rate inside/outside the play tolerance if ( f1 >= kp->acc_play_radius ) { //----- Apply 100% tracking sensitivity if outside play tolerance f1 = 1.0f ; } else { //----- If inside play tolerance, weaken tracking sensitivity as target gets closer f1 /= kp->acc_play_radius ; f1 *= f1 ; // Second power f1 *= f1 ; // Fourth power } f1 *= kp->acc_sensitivity ; //----- Tracking *acc += f1 * f2 ; } else { if ( f2 < -kp->acc_play_radius ) { *acc += ( f2 + kp->acc_play_radius ) * kp->acc_sensitivity ; } else if ( f2 > kp->acc_play_radius ) { *acc += ( f2 - kp->acc_play_radius ) * kp->acc_sensitivity ; } } } /******************************************************************************* Calculate Controller Tilt from Acceleration *******************************************************************************/ static void calc_acc_horizon( KPADInsideStatus *kp ) { f32 f1, vx,vy, ax,ay ; //----- XY acceleration normalization f1 = sqrtf( kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ; if ( f1 == 0.0f || f1 >= 2.0f ) return ; ax = kp->hard_acc.x / f1 ; ay = kp->hard_acc.y / f1 ; //----- There will be more power the closer the XY acceleration length is to one (1). if ( f1 > 1.0f ) { f1 = 2.0f - f1 ; } f1 *= f1 * kp_acc_horizon_pw ; //----- Target tilt vx = kp->accXY_nrm_hori.x * ax + kp->accXY_nrm_hori.y * ay ; vy = kp->accXY_nrm_hori.y * ax - kp->accXY_nrm_hori.x * ay ; //----- Set closer ax = ( vx - kp->acc_horizon.x ) * f1 + kp->acc_horizon.x ; ay = ( vy - kp->acc_horizon.y ) * f1 + kp->acc_horizon.y ; //----- Normalization f1 = sqrtf( ax * ax + ay * ay ) ; if ( f1 == 0.0f ) return ; kp->acc_horizon.x = ax / f1 ; kp->acc_horizon.y = ay / f1 ; //----- Update static determination coordinate kp->ah_circle_pos.x += ( kp->acc_horizon.x - kp->ah_circle_pos.x ) * kp_ah_circle_pw ; kp->ah_circle_pos.y += ( kp->acc_horizon.y - kp->ah_circle_pos.y ) * kp_ah_circle_pw ; vx = kp->acc_horizon.x - kp->ah_circle_pos.x ; vy = kp->acc_horizon.y - kp->ah_circle_pos.y ; if ( vx*vx + vy*vy <= kp->ah_circle_radius2 ) { if ( kp->ah_circle_ct ) -- kp->ah_circle_ct ; } else { kp->ah_circle_ct = kp_ah_circle_ct ; } } static void calc_acc_vertical( KPADInsideStatus *kp ) { KPADStatus *sp = &kp->status ; f32 f1,f2, ax,ay ; //----- Target tilt ax = sqrtf( f2 = kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ; ay = - kp->hard_acc.z ; f1 = sqrtf( f2 + ay * ay ) ; if ( f1 == 0.0f || f1 >= 2.0f ) return ; ax /= f1 ; ay /= f1 ; //----- There will be more power the closer the acceleration length is to one (1). if ( f1 > 1.0f ) { f1 = 2.0f - f1 ; } f1 *= f1 * kp_acc_horizon_pw ; //----- Set closer ax = ( ax - sp->acc_vertical.x ) * f1 + sp->acc_vertical.x ; ay = ( ay - sp->acc_vertical.y ) * f1 + sp->acc_vertical.y ; //----- Normalization f1 = sqrtf( ax * ax + ay * ay ) ; if ( f1 == 0.0f ) return ; sp->acc_vertical.x = ax / f1 ; sp->acc_vertical.y = ay / f1 ; } /******************************************************************************* KPAD Acceleration Information Loading *******************************************************************************/ static f32 clamp_acc( f32 acc, f32 clamp ) { if ( acc < 0.0f ) { if ( acc < -clamp ) return ( -clamp ) ; } else { if ( acc > clamp ) return ( clamp ) ; } return ( acc ) ; } static void read_kpad_acc( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp ) { KPADStatus *sp = &kp->status ; Vec fsrc ; Vec vec ; switch ( uwp->fmt ) { case WPAD_FMT_CORE_ACC : case WPAD_FMT_CORE_ACC_DPD : case WPAD_FMT_FREESTYLE_ACC : case WPAD_FMT_FREESTYLE_ACC_DPD : case WPAD_FMT_CLASSIC_ACC : case WPAD_FMT_CLASSIC_ACC_DPD : case WPAD_FMT_GUITAR : // Core ACC is OK break ; default : return ; } //----- Update raw values kp->hard_acc.x = clamp_acc( (f32)(s32)-uwp->u.core.accX * kp->acc_scale_x, kp_rm_acc_max ) ; kp->hard_acc.y = clamp_acc( (f32)(s32)-uwp->u.core.accZ * kp->acc_scale_z, kp_rm_acc_max ) ; kp->hard_acc.z = clamp_acc( (f32)(s32) uwp->u.core.accY * kp->acc_scale_y, kp_rm_acc_max ) ; //----- Temporary save vec = sp->acc ; //----- Acceleration tracking process for application use calc_acc( kp, &sp->acc.x, kp->hard_acc.x ) ; calc_acc( kp, &sp->acc.y, kp->hard_acc.y ) ; calc_acc( kp, &sp->acc.z, kp->hard_acc.z ) ; sp->acc_value = sqrtf( sp->acc.x * sp->acc.x + sp->acc.y * sp->acc.y + sp->acc.z * sp->acc.z ) ; //----- Amount of change in acceleration, for application use vec.x -= sp->acc.x ; vec.y -= sp->acc.y ; vec.z -= sp->acc.z ; sp->acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ; //----- Calculate controller tilt from the raw acceleration calc_acc_horizon( kp ) ; calc_acc_vertical( kp ) ; /*********************************************************************** Load Nunchuk unit acceleration ***********************************************************************/ if ( uwp->u.fs.err != WPAD_ERR_NONE || uwp->u.fs.dev != WPAD_DEV_FREESTYLE || (uwp->fmt != WPAD_FMT_FREESTYLE_ACC && uwp->fmt != WPAD_FMT_FREESTYLE_ACC_DPD) ) { return ; } fsrc.x = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccX * kp->fs_acc_scale_x, kp_fs_acc_max ) ; fsrc.y = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccZ * kp->fs_acc_scale_z, kp_fs_acc_max ) ; fsrc.z = clamp_acc( (f32)(s32) uwp->u.fs.fsAccY * kp->fs_acc_scale_y, kp_fs_acc_max ) ; if ( kp->fsAccRevise ) { MTXMultVec( kp_fs_rot, &fsrc, &fsrc ) ; } //----- Temporary save vec = sp->ex_status.fs.acc ; //----- Acceleration tracking process for application use calc_acc( kp, &sp->ex_status.fs.acc.x, fsrc.x ) ; calc_acc( kp, &sp->ex_status.fs.acc.y, fsrc.y ) ; calc_acc( kp, &sp->ex_status.fs.acc.z, fsrc.z ) ; 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 ) ; //----- Amount of change in acceleration, for application use vec.x -= sp->ex_status.fs.acc.x ; vec.y -= sp->ex_status.fs.acc.y ; vec.z -= sp->ex_status.fs.acc.z ; sp->ex_status.fs.acc_speed = sqrtf( vec.x * vec.x + vec.y * vec.y + vec.z * vec.z ) ; } /******************************************************************************* Change WPAD object to KPAD *******************************************************************************/ static void get_kobj( KPADInsideStatus *kp, DPDObject *wobj_p ) { const f32 dpd_scale = 2.0f / (f32)KPAD_DPD_RESO_WX ; const f32 dpd_cx = (f32)( KPAD_DPD_RESO_WX - 1 ) / (f32)KPAD_DPD_RESO_WX ; const f32 dpd_cy = (f32)( KPAD_DPD_RESO_WY - 1 ) / (f32)KPAD_DPD_RESO_WX ; KPADObject *kobj_p ; //----- Store kobj_p = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { if ( wobj_p->size ) { //----- Valid object kobj_p->center.x = (f32)(s32)wobj_p->x * dpd_scale - dpd_cx ; kobj_p->center.y = (f32)(s32)wobj_p->y * dpd_scale - dpd_cy ; kobj_p->error_fg = 0 ; // Applied kobj_p->state_fg = 0 ; // Normal } else { //----- Invalid object kobj_p->error_fg = -1 ; // Not applied } -- wobj_p ; } while ( --kobj_p >= kp->kobj_sample ) ; } /******************************************************************************* Set Surrounding Objects to Invalid *******************************************************************************/ static void check_kobj_outside_frame( KPADInsideStatus *kp, KPADObject *kobj_t ) { KPADObject *kobj_p = &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { if ( kobj_p->error_fg < 0 ) continue ; if ( kobj_p->center.x <= kp->kobj_frame_min.x || kobj_p->center.x >= kp->kobj_frame_max.x || kobj_p->center.y <= kp->kobj_frame_min.y || kobj_p->center.y >= kp->kobj_frame_max.y ) { kobj_p->error_fg |= 1 ; } } while ( --kobj_p >= kobj_t ) ; } /******************************************************************************* Set Objects at Same Coordinates to Invalid *******************************************************************************/ static void check_kobj_same_position( KPADObject *kobj_t ) { KPADObject *op1, *op2 ; op1 = kobj_t ; do { if ( op1->error_fg != 0 ) continue ; op2 = op1 + 1 ; do { if ( op2->error_fg != 0 ) continue ; if ( op1->center.x == op2->center.x && op1->center.y == op2->center.y ) { op2->error_fg |= 2 ; // Set just one as error } } while ( ++op2 <= &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } while ( ++op1 < &kobj_t[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } /******************************************************************************* Calculate Controller Tilt from Two Points (return the distance from TV) *******************************************************************************/ static f32 calc_horizon( KPADInsideStatus *kp, Vec2 *p1, Vec2 *p2, Vec2 *hori ) { f32 f1, vx,vy ; vx = p2->x - p1->x ; vy = p2->y - p1->y ; f1 = 1.0f / sqrtf( vx * vx + vy * vy ) ; // Should not be zero vx *= f1 ; vy *= f1 ; hori->x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ; hori->y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ; return ( kp->dist_vv1 * f1 ) ; } /******************************************************************************* Select Two Marks for the First Time *******************************************************************************/ static s8 select_2obj_first( KPADInsideStatus *kp ) { KPADObject *op1,*op2, *rp1,*rp2 ; Vec2 hori ; f32 f1, max = kp_err_first_inpr ; op1 = kp->kobj_sample ; do { if ( op1->error_fg != 0 ) continue ; op2 = op1 + 1 ; do { if ( op2->error_fg != 0 ) continue ; f1 = calc_horizon( kp, &op1->center, &op2->center, &hori ) ; //----- Operational distance range check if ( f1 <= kp->err_dist_min || f1 >= kp_err_dist_max ) continue ; f1 = kp->acc_horizon.x * hori.x + kp->acc_horizon.y * hori.y ; if ( f1 < 0.0f ) { if ( -f1 > max ) { max = -f1 ; rp1 = op2 ; rp2 = op1 ; } } else { if ( f1 > max ) { max = f1 ; rp1 = op1 ; rp2 = op2 ; } } } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; //----- Confirmed regular mark? if ( max == kp_err_first_inpr ) return ( 0 ) ; kp->kobj_regular[ 0 ] = *rp1 ; kp->kobj_regular[ 1 ] = *rp2 ; return ( 2 ) ; // Two points recognition } /******************************************************************************* Continuing, select two marks using only the interval information *******************************************************************************/ static s8 select_2obj_continue( KPADInsideStatus *kp ) { KPADObject *op1,*op2, *rp1,*rp2 ; Vec2 nrm ; s32 rev_fg ; f32 f1,f2, vx,vy, min = 2.0f ; //----- Find two points closest to the previous tilt and distance op1 = kp->kobj_sample ; do { if ( op1->error_fg != 0 ) continue ; op2 = op1 + 1 ; do { if ( op2->error_fg != 0 ) continue ; //----- Direction calculation vx = op2->center.x - op1->center.x ; vy = op2->center.y - op1->center.y ; f1 = 1.0f / sqrtf( vx*vx + vy*vy ) ; // Should not be zero. nrm.x = vx * f1 ; nrm.y = vy * f1 ; //----- Operational distance range check f1 *= kp->dist_vv1 ; // Distance if ( f1 <= kp->err_dist_min || f1 >= kp_err_dist_max ) continue ; //----- Check amount of change in distance f1 -= kp->sec_dist ; if ( f1 < 0.0f ) { f1 *= kp->err_dist_speedM_1 ; } else { f1 *= kp->err_dist_speed_1 ; } if ( f1 >= 1.0f ) continue ; // Distance error rate //----- Check amount of change in tilt f2 = kp->sec_nrm.x * nrm.x + kp->sec_nrm.y * nrm.y ; if ( f2 < 0.0f ) { f2 = -f2 ; rev_fg = 1 ; // Handle with orientation inverted (op2 -> op1) } else { rev_fg = 0 ; // Handle as is (op1 -> op2) } if ( f2 <= kp_err_next_inpr ) continue ; f2 = ( 1.0f - f2 ) / ( 1.0f - kp_err_next_inpr ) ; // Tilt error rate //----- Record the object with the smallest error f1 += f2 ; // Determine through the sum of distance error rate and tilt error rate if ( f1 < min ) { min = f1 ; if ( rev_fg ) { rp1 = op2 ; rp2 = op1 ; } else { rp1 = op1 ; rp2 = op2 ; } } } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; //----- Confirmed regular mark? if ( min == 2.0f ) return ( 0 ) ; kp->kobj_regular[ 0 ] = *rp1 ; kp->kobj_regular[ 1 ] = *rp2 ; return ( 2 ) ; // Two points recognition } /******************************************************************************* Select One Mark for First Time *******************************************************************************/ static s8 select_1obj_first( KPADInsideStatus *kp ) { KPADObject *op1 ; f32 vx,vy ; Vec2 p1,p2 ; //----- Determine the section direction vx = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ; vy = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ; //----- Determine the section vector vx *= kp->trust_sec_length ; vy *= kp->trust_sec_length ; //----- Search for a point where the expected point can be outside op1 = kp->kobj_sample ; do { if ( op1->error_fg != 0 ) continue ; p1.x = op1->center.x - vx ; // Expected point is to the left p1.y = op1->center.y - vy ; p2.x = op1->center.x + vx ; // Expected point is to the right p2.y = op1->center.y + vy ; if ( p1.x <= kp->kobj_frame_min.x || p1.x >= kp->kobj_frame_max.x || p1.y <= kp->kobj_frame_min.y || p1.y >= kp->kobj_frame_max.y ) { //----- If the left expected point is outside, the right expected point needs to be inside if ( p2.x > kp->kobj_frame_min.x && p2.x < kp->kobj_frame_max.x && p2.y > kp->kobj_frame_min.y && p2.y < kp->kobj_frame_max.y ) { //----- op1 may be the right mark kp->kobj_regular[ 1 ] = *op1 ; kp->kobj_regular[ 0 ].center = p1 ; kp->kobj_regular[ 0 ].error_fg = 0 ; kp->kobj_regular[ 0 ].state_fg = -1 ; return ( -1 ) ; // Recognize one unstable point } } else { //----- If the left expected point is inside, the right expected point needs to be outside. if ( p2.x <= kp->kobj_frame_min.x || p2.x >= kp->kobj_frame_max.x || p2.y <= kp->kobj_frame_min.y || p2.y >= kp->kobj_frame_max.y ) { //----- op1 may be the left mark kp->kobj_regular[ 0 ] = *op1 ; kp->kobj_regular[ 1 ].center = p2 ; kp->kobj_regular[ 1 ].error_fg = 0 ; kp->kobj_regular[ 1 ].state_fg = -1 ; return ( -1 ) ; // Recognize one unstable point } } } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ; return ( 0 ) ; } /******************************************************************************* Select One Mark Subsequently *******************************************************************************/ static s8 select_1obj_continue( KPADInsideStatus *kp ) { KPADObject *op1,*op2, *rp1,*rp2 ; f32 f1, vx,vy ; f32 min = kp_err_near_pos * kp_err_near_pos ; //----- Select the mark closest to the past regular mark op1 = kp->kobj_regular ; do { if ( op1->error_fg != 0 ) continue ; if ( op1->state_fg != 0 ) continue ; // No expected points op2 = kp->kobj_sample ; do { if ( op2->error_fg != 0 ) continue ; vx = op1->center.x - op2->center.x ; vy = op1->center.y - op2->center.y ; f1 = vx * vx + vy * vy ; if ( f1 < min ) { min = f1 ; rp1 = op1 ; rp2 = op2 ; } } while ( ++op2 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ; } while ( ++op1 < &kp->kobj_regular[ KPAD_USE_OBJECTS ] ) ; //----- Confirmed regular mark? if ( min == kp_err_near_pos * kp_err_near_pos ) return ( 0 ) ; *rp1 = *rp2 ; //----- Calculate tilt from the acceleration kp->sec_nrm.x = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ; kp->sec_nrm.y = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ; //----- Calculate expected point coordinates vx = kp->sec_length * kp->sec_nrm.x ; vy = kp->sec_length * kp->sec_nrm.y ; if ( rp1 == &kp->kobj_regular[ 0 ] ) { kp->kobj_regular[ 1 ].center.x = rp1->center.x + vx ; kp->kobj_regular[ 1 ].center.y = rp1->center.y + vy ; kp->kobj_regular[ 1 ].error_fg = 0 ; kp->kobj_regular[ 1 ].state_fg = -1 ; } else { kp->kobj_regular[ 0 ].center.x = rp1->center.x - vx ; kp->kobj_regular[ 0 ].center.y = rp1->center.y - vy ; kp->kobj_regular[ 0 ].error_fg = 0 ; kp->kobj_regular[ 0 ].state_fg = -1 ; } if ( kp->status.dpd_valid_fg < 0 ) { return ( -1 ) ; // Recognize one unstable point } else { return ( 1 ) ; // Recognize one point } } /******************************************************************************* Calculate Controller Tilt from Object *******************************************************************************/ static void calc_obj_horizon( KPADInsideStatus *kp ) { f32 f1, vx,vy ; vx = kp->kobj_regular[ 1 ].center.x - kp->kobj_regular[ 0 ].center.x ; vy = kp->kobj_regular[ 1 ].center.y - kp->kobj_regular[ 0 ].center.y ; kp->sec_length = sqrtf( vx * vx + vy * vy ) ; // Should not be zero f1 = 1.0f / kp->sec_length ; kp->sec_dist = kp->dist_vv1 * f1 ; kp->sec_nrm.x = ( vx *= f1 ) ; kp->sec_nrm.y = ( vy *= f1 ) ; kp->obj_horizon.x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ; kp->obj_horizon.y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ; } /******************************************************************************* Update Application Variables *******************************************************************************/ static void calc_dpd_variable( KPADInsideStatus *kp, s8 valid_fg_next ) { KPADStatus *sp = &kp->status ; f32 f1,f2, dist ; Vec2 pos, vec ; if ( valid_fg_next == 0 ) { sp->dpd_valid_fg = 0 ; return ; } /*********************************************************************** Calculate Controller Tilt ***********************************************************************/ //----- Calculate the target value pos.x = kp->sec_nrm_hori.x * kp->sec_nrm.x + kp->sec_nrm_hori.y * kp->sec_nrm.y ; pos.y = kp->sec_nrm_hori.y * kp->sec_nrm.x - kp->sec_nrm_hori.x * kp->sec_nrm.y ; //----- Consider the tracking sensitivity and play tolerance for the target value if ( sp->dpd_valid_fg == 0 ) { //----- Because this is the first pointing, initialize with the given values sp->horizon = pos ; sp->hori_vec = Vec2_0 ; sp->hori_speed = 0.0f ; } else { //----- Difference to the target value vec.x = pos.x - sp->horizon.x ; vec.y = pos.y - sp->horizon.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; if ( kp->hori_play_mode == KPAD_PLAY_MODE_LOOSE ) { //----- Calculation of tracking rate inside/outside the play tolerance if ( f1 >= kp->hori_play_radius ) { //----- Apply 100% tracking sensitivity if outside play tolerance f1 = 1.0f ; } else { //----- If inside play tolerance, weaken tracking sensitivity as target gets closer f1 /= kp->hori_play_radius ; f1 *= f1 ; // Second power f1 *= f1 ; // Fourth power } f1 *= kp->hori_sensitivity ; //----- Tracking vec.x = f1 * vec.x + sp->horizon.x ; vec.y = f1 * vec.y + sp->horizon.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; // Normalize because this is tilt vec.x /= f1 ; vec.y /= f1 ; sp->hori_vec.x = vec.x - sp->horizon.x ; sp->hori_vec.y = vec.y - sp->horizon.y ; sp->hori_speed = sqrtf( sp->hori_vec.x * sp->hori_vec.x + sp->hori_vec.y * sp->hori_vec.y ) ; sp->horizon = vec ; } else { if ( f1 > kp->hori_play_radius ) { //----- Track because this is outside the play tolerance f1 = ( f1 - kp->hori_play_radius ) / f1 * kp->hori_sensitivity ; vec.x = vec.x * f1 + sp->horizon.x ; vec.y = vec.y * f1 + sp->horizon.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; vec.x /= f1 ; vec.y /= f1 ; sp->hori_vec.x = vec.x - sp->horizon.x ; sp->hori_vec.y = vec.y - sp->horizon.y ; sp->hori_speed = sqrtf( sp->hori_vec.x * sp->hori_vec.x + sp->hori_vec.y * sp->hori_vec.y ) ; sp->horizon = vec ; } else { //----- Do not move this because it is inside the play tolerance sp->hori_vec = Vec2_0 ; sp->hori_speed = 0.0f ; } } } /*********************************************************************** Calculate Distance from TV ***********************************************************************/ //----- Calculate the target value dist = kp->dist_vv1 / kp->sec_length ; //----- Consider the tracking sensitivity and play tolerance for the target value if ( sp->dpd_valid_fg == 0 ) { //----- Because this is the first pointing, initialize with the given values sp->dist = dist ; sp->dist_vec = 0.0f ; sp->dist_speed = 0.0f ; } else { //----- Difference to the target value f2 = dist - sp->dist ; if ( f2 < 0.0f ) { f1 = -f2 ; } else { f1 = f2 ; } if ( kp->dist_play_mode == KPAD_PLAY_MODE_LOOSE ) { //----- Calculation of tracking rate inside/outside the play tolerance if ( f1 >= kp->dist_play_radius ) { //----- Apply 100% tracking sensitivity if outside play tolerance f1 = 1.0f ; } else { //----- If inside play tolerance, weaken tracking sensitivity as target gets closer f1 /= kp->dist_play_radius ; f1 *= f1 ; // Second power f1 *= f1 ; // Fourth power } f1 *= kp->dist_sensitivity ; //----- Tracking sp->dist_vec = f1 * f2 ; if ( sp->dist_vec < 0.0f ) { sp->dist_speed = -sp->dist_vec ; } else { sp->dist_speed = sp->dist_vec ; } sp->dist += sp->dist_vec ; } else { if ( f1 > kp->dist_play_radius ) { //----- Track because this is outside the play tolerance f1 = ( f1 - kp->dist_play_radius ) / f1 * kp->dist_sensitivity ; sp->dist_vec = f1 * f2 ; if ( sp->dist_vec < 0.0f ) { sp->dist_speed = -sp->dist_vec ; } else { sp->dist_speed = sp->dist_vec ; } sp->dist += sp->dist_vec ; } else { //----- Do not move this because it is inside the play tolerance sp->dist_vec = 0.0f ; sp->dist_speed = 0.0f ; } } } /*********************************************************************** Calculate Pointing Location ***********************************************************************/ //----- Center coordinates of two marks pos.x = ( kp->kobj_regular[ 0 ].center.x + kp->kobj_regular[ 1 ].center.x ) * 0.5f ; pos.y = ( kp->kobj_regular[ 0 ].center.y + kp->kobj_regular[ 1 ].center.y ) * 0.5f ; //----- Rotate by the amount of twist f1 = kp->sec_nrm.x * kp->sec_nrm_hori.x + kp->sec_nrm.y * kp->sec_nrm_hori.y ; f2 = -kp->sec_nrm.y * kp->sec_nrm_hori.x + kp->sec_nrm.x * kp->sec_nrm_hori.y ; vec.x = f1 * pos.x - f2 * pos.y ; vec.y = f2 * pos.x + f1 * pos.y ; //----- Apply scaling after correcting the center position vec.x = ( kp->center_org.x - vec.x ) * kp->dpd2pos_scale ; vec.y = ( kp->center_org.y - vec.y ) * kp->dpd2pos_scale ; //----- Convert to the gravitational direction coordinate system during calibration pos.x = -kp->accXY_nrm_hori.y * vec.x + kp->accXY_nrm_hori.x * vec.y ; pos.y = -kp->accXY_nrm_hori.x * vec.x - kp->accXY_nrm_hori.y * vec.y ; //----- Consider the tracking sensitivity and play tolerance for the target value if ( sp->dpd_valid_fg == 0 ) { //----- Because this is the first pointing, initialize with the given values sp->pos = pos ; sp->vec = Vec2_0 ; sp->speed = 0.0f ; } else { //----- Difference to the target value vec.x = pos.x - sp->pos.x ; vec.y = pos.y - sp->pos.y ; f1 = sqrtf( vec.x * vec.x + vec.y * vec.y ) ; if ( kp->pos_play_mode == KPAD_PLAY_MODE_LOOSE ) { //----- Calculation of tracking rate inside/outside the play tolerance if ( f1 >= kp->pos_play_radius ) { //----- Apply 100% tracking sensitivity if outside play tolerance f1 = 1.0f ; } else { //----- If inside play tolerance, weaken tracking sensitivity as target gets closer f1 /= kp->pos_play_radius ; f1 *= f1 ; // Second power f1 *= f1 ; // Fourth power } f1 *= kp->pos_sensitivity ; //----- Tracking sp->vec.x = f1 * vec.x ; sp->vec.y = f1 * vec.y ; sp->speed = sqrtf( sp->vec.x * sp->vec.x + sp->vec.y * sp->vec.y ) ; sp->pos.x += sp->vec.x ; sp->pos.y += sp->vec.y ; } else { if ( f1 > kp->pos_play_radius ) { //----- Track because this is outside the play tolerance f1 = ( f1 - kp->pos_play_radius ) / f1 * kp->pos_sensitivity ; sp->vec.x = f1 * vec.x ; sp->vec.y = f1 * vec.y ; sp->speed = sqrtf( sp->vec.x * sp->vec.x + sp->vec.y * sp->vec.y ) ; sp->pos.x += sp->vec.x ; sp->pos.y += sp->vec.y ; } else { //----- Do not move this because it is inside the play tolerance sp->vec = Vec2_0 ; sp->speed = 0.0f ; } } } /*********************************************************************** Update Flags ***********************************************************************/ sp->dpd_valid_fg = valid_fg_next ; } /******************************************************************************* KPAD DPD Information Loading *******************************************************************************/ static void read_kpad_dpd( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp ) { KPADStatus *sp = &kp->status ; KPADObject *op1 ; s8 valid_fg_next ; /*********************************************************************** Change WPAD object to KPAD ***********************************************************************/ if ( uwp->fmt == WPAD_FMT_CORE_ACC_DPD || uwp->fmt == WPAD_FMT_FREESTYLE_ACC_DPD || uwp->fmt == WPAD_FMT_CLASSIC_ACC_DPD || ( uwp->fmt == WPAD_FMT_GUITAR && kp->dpdCurrState ) ) { //----- Change the WPAD object to KPAD get_kobj( kp, &uwp->u.core.obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; } else { // dpd data is not prepared op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { op1->error_fg = -1 ; } while ( --op1 >= kp->kobj_sample ) ; } /*********************************************************************** Select the normal object ***********************************************************************/ //----- Remove untrustworthy objects check_kobj_outside_frame( kp, kp->kobj_sample ) ; // Set Surrounding Objects to Invalid check_kobj_same_position( kp->kobj_sample ) ; // Set the objects at the same coordinates to invalid //----- Determine how many are being applied kp->valid_objs = 0 ; op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; do { if ( op1->error_fg == 0 ) ++ kp->valid_objs ; } while ( --op1 >= kp->kobj_sample ) ; //----- Recognition processing if ( sp->acc_vertical.x <= kp_err_up_inpr ) goto LABEL_select_NG ; if ( sp->dpd_valid_fg == 2 || sp->dpd_valid_fg == -2 ) { //----- Recognized using two objects from the previous time if ( kp->valid_objs >= 2 ) { valid_fg_next = select_2obj_continue( kp ) ; if ( valid_fg_next ) goto LABEL_select_OK ; } if ( kp->valid_objs >= 1 ) { valid_fg_next = select_1obj_continue( kp ) ; if ( valid_fg_next ) goto LABEL_select_OK ; } } else if ( sp->dpd_valid_fg == 1 || sp->dpd_valid_fg == -1 ) { //----- Recognized using one object from the previous time if ( kp->valid_objs >= 2 ) { valid_fg_next = select_2obj_first( kp ) ; if ( valid_fg_next ) goto LABEL_select_OK ; } if ( kp->valid_objs >= 1 ) { valid_fg_next = select_1obj_continue( kp ) ; if ( valid_fg_next ) goto LABEL_select_OK ; } } else { //----- Not recognized the previous time if ( kp->valid_objs >= 2 ) { valid_fg_next = select_2obj_first( kp ) ; if ( valid_fg_next ) goto LABEL_select_OK ; } if ( kp->valid_objs == 1 ) { valid_fg_next = select_1obj_first( kp ) ; if ( valid_fg_next ) goto LABEL_select_OK ; } } LABEL_select_NG : valid_fg_next = 0 ; // Was not able to select LABEL_select_OK : // Maybe was able to select //----- Update section information if selected if ( valid_fg_next ) { //----- Calculate the information of two points and object tilt calc_obj_horizon( kp ) ; //----- Error if obviously different from the acceleration tilt if ( kp->ah_circle_ct == 0 ) { if ( kp->obj_horizon.x * kp->acc_horizon.x + kp->obj_horizon.y * kp->acc_horizon.y <= kp_err_acc_inpr ) { valid_fg_next = 0 ; // Invalid after all kp->kobj_regular[ 0 ].error_fg = kp->kobj_regular[ 1 ].error_fg = 1 ; } } //----- Consecutive two point recognition count if ( sp->dpd_valid_fg == 2 && valid_fg_next == 2 ) { if ( kp->dpd_valid2_ct == 200 ) { kp->trust_sec_length = kp->sec_length ; } else { ++ kp->dpd_valid2_ct ; } } else { kp->dpd_valid2_ct = 0 ; } } else { kp->dpd_valid2_ct = 0 ; } //----- Update application variables calc_dpd_variable( kp, valid_fg_next ) ; } /******************************************************************************* Clamp Processing of Analog Triggers *******************************************************************************/ static void clamp_trigger( f32 *trigger, s32 tr, s32 min, s32 max ) { if ( tr <= min ) { *trigger = 0.0f ; } else if ( tr >= max ) { *trigger = 1.0f ; } else { *trigger = (f32)( tr - min ) / (f32)( max - min ) ; } } /******************************************************************************* Clamp Processing of Sticks *******************************************************************************/ static void clamp_stick_circle( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max ) { f32 length ; f32 fx = (f32)sx ; f32 fy = (f32)sy ; f32 fmin = (f32)min ; f32 fmax = (f32)max ; length = sqrtf( fx * fx + fy * fy ) ; if ( length <= fmin ) { stick->x = stick->y = 0.0f ; } else if ( length >= fmax ) { stick->x = fx / length ; stick->y = fy / length ; } else { length = ( length - fmin ) / ( fmax - fmin ) / length ; stick->x = fx * length ; stick->y = fy * length ; } } static void clamp_stick_cross( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max ) { f32 length ; //----- Process clamp for each axis separately if ( sx < 0 ) { clamp_trigger( &stick->x, -sx, min, max ) ; stick->x = -stick->x ; } else { clamp_trigger( &stick->x, sx, min, max ) ; } if ( sy < 0 ) { clamp_trigger( &stick->y, -sy, min, max ) ; stick->y = -stick->y ; } else { clamp_trigger( &stick->y, sy, min, max ) ; } //----- Set maximum length to 1 length = stick->x * stick->x + stick->y * stick->y ; if ( length > 1.0f ) { length = sqrtf( length ) ; stick->x /= length ; stick->y /= length ; } } /******************************************************************************* Load stick information *******************************************************************************/ static void read_kpad_ext( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp ) { KPADEXStatus *ep = &kp->status.ex_status ; void (*clampStickFuncp)( Vec2 *stick, s32 sx, s32 sy, s32 min, s32 max ) ; int idx ; clampStickFuncp = kp_stick_clamp_cross ? clamp_stick_cross : clamp_stick_circle ; if ( uwp->u.fs.dev == WPAD_DEV_FREESTYLE && (uwp->fmt == WPAD_FMT_FREESTYLE || uwp->fmt == WPAD_FMT_FREESTYLE_ACC || uwp->fmt == WPAD_FMT_FREESTYLE_ACC_DPD) ) { if ( kp->exResetReq ) { kp->exResetReq = FALSE ; //----- Nunchuk unit ep->fs.stick = Vec2_0 ; ep->fs.acc.x = ep->fs.acc.z = 0.0f ; ep->fs.acc.y = -1.0f ; ep->fs.acc_value = 1.0f ; ep->fs.acc_speed = 0.0f ; } //----- Stick data processing clampStickFuncp( &ep->fs.stick, uwp->u.fs.fsStickX, uwp->u.fs.fsStickY, kp_fs_fstick_min, kp_fs_fstick_max ) ; } else if ( uwp->u.cl.dev == WPAD_DEV_CLASSIC && (uwp->fmt == WPAD_FMT_CLASSIC || uwp->fmt == WPAD_FMT_CLASSIC_ACC || uwp->fmt == WPAD_FMT_CLASSIC_ACC_DPD) ) { if ( kp->exResetReq ) { kp->exResetReq = FALSE ; //----- Classic Controller unit ep->cl.lstick = Vec2_0 ; ep->cl.rstick = Vec2_0 ; ep->cl.ltrigger = ep->cl.rtrigger = 0.0f ; ep->cl.hold = ep->cl.trig = ep->cl.release = 0x00000000 ; kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_repeat_delay ; } //----- Stick data processing clampStickFuncp( &ep->cl.lstick, uwp->u.cl.clLStickX, uwp->u.cl.clLStickY, kp_cl_stick_min, kp_cl_stick_max ) ; clampStickFuncp( &ep->cl.rstick, uwp->u.cl.clRStickX, uwp->u.cl.clRStickY, kp_cl_stick_min, kp_cl_stick_max ) ; clamp_trigger( &ep->cl.ltrigger, uwp->u.cl.clTriggerL, kp_cl_trigger_min, kp_cl_trigger_max ) ; clamp_trigger( &ep->cl.rtrigger, uwp->u.cl.clTriggerR, kp_cl_trigger_min, kp_cl_trigger_max ) ; } else if ( (uwp->u.cl.dev == WPAD_DEV_GUITAR && uwp->fmt == WPAD_FMT_GUITAR) ) { if ( kp->exResetReq ) { kp->exResetReq = FALSE ; //------ Guitar or drum unit ep->cl.lstick = Vec2_0 ; ep->cl.rstick = Vec2_0 ; ep->cl.ltrigger = ep->cl.rtrigger = 0.0f ; ep->cl.hold = ep->cl.trig = ep->cl.release = 0x00000000 ; kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_repeat_delay ; } //----- Stick data processing clampStickFuncp( &ep->cl.lstick, uwp->u.cl.clLStickX, uwp->u.cl.clLStickY, kp_cl_stick_min, kp_cl_stick_max ) ; clamp_trigger( &ep->cl.rstick.x, uwp->u.cl.clRStickX, kp_ex_analog_min, kp_ex_analog_max ) ; clamp_trigger( &ep->cl.rstick.y, uwp->u.cl.clRStickY, kp_ex_analog_min, kp_ex_analog_max ) ; clamp_trigger( &ep->cl.ltrigger, uwp->u.cl.clTriggerL, kp_ex_trigger_min, kp_ex_trigger_max ) ; clamp_trigger( &ep->cl.rtrigger, uwp->u.cl.clTriggerR, kp_ex_trigger_min, kp_ex_trigger_max ) ; } else if ( uwp->u.cl.dev == WPAD_DEV_TRAIN && uwp->fmt == WPAD_FMT_TRAIN ) { if ( kp->exResetReq ) { kp->exResetReq = FALSE ; //----- Master controller unit ep->cl.lstick = Vec2_0 ; ep->cl.rstick = Vec2_0 ; ep->cl.ltrigger = ep->cl.rtrigger = 0.0f ; ep->cl.hold = ep->cl.trig = ep->cl.release = 0x00000000 ; kp->btn_cl_repeat_time = 0 ; kp->btn_cl_repeat_next = kp->btn_repeat_delay ; } //----- Stick data processing ep->cl.lstick.x = ep->cl.lstick.y = 0.0f ; ep->cl.rstick.x = ep->cl.rstick.y = 0.0f ; clamp_trigger( &ep->cl.ltrigger, uwp->u.tr.brake, kp_ex_trigger_min, kp_ex_trigger_max ) ; clamp_trigger( &ep->cl.rtrigger, uwp->u.tr.mascon, kp_ex_trigger_min, kp_ex_trigger_max ) ; } else if ( uwp->u.bl.dev == WPAD_DEV_BALANCE_CHECKER && uwp->fmt == WPAD_FMT_BALANCE_CHECKER ) { if ( kp->exResetReq ) { kp->exResetReq = FALSE ; KPADResetWbcZeroPoint() ; } //----- Weight data processing if ( WBCGetBatteryLevel( uwp->u.bl.battery ) == 0 ) { //----- Error when the remaining battery life is below a fixed amount and the Wii Balance Board has ceased to function ep->bl.weight_err = KPAD_WBC_ERR_NO_BATTERY ; } else if ( kp_wbc_zero_point_done < 3 ) { //----- Cannot be used until initial settings are complete ep->bl.weight_err = KPAD_WBC_ERR_SETUP ; } else if ( uwp->u.bl.temp == 127 || uwp->u.bl.temp == -128 ) { //----- Error when temperature information is incorrect ep->bl.weight_err = KPAD_WBC_ERR_WRONG_TEMP ; } else { //----- weight update ep->bl.weight_err = WBCRead( &( uwp->u.bl ), ep->bl.weight, WPAD_PRESS_UNITS ) ; if ( ep->bl.weight_err >= 0 ) { for ( idx = 0 ; idx < WPAD_PRESS_UNITS ; idx ++ ) { kp_wbc_weight_ave[ idx ] = ( kp_wbc_weight_ave[ idx ] * kp_wbc_ave_count + ep->bl.weight[ idx ] ) / ( kp_wbc_ave_count + 1 ) ; } if ( kp_wbc_tgc_weight_issued ) { //----- Calculate the average value over two seconds kp_wbc_ave_sample_count++ ; for ( idx = 0 ; idx < WPAD_PRESS_UNITS ; idx ++ ) { kp_wbc_ave_sample[ idx ] = ( kp_wbc_ave_sample[ idx ] * ( kp_wbc_ave_sample_count - 1 ) + ep->bl.weight[ idx ] ) / kp_wbc_ave_sample_count ; } if ( kp_wbc_ave_sample_count == kp_wbc_ave_count ) { //----- Calculate the body weight with correction applied because the average value over two seconds was found kp_wbc_tgc_weight_issued = 0 ; //----- Incorrect temperature information caused the error in this case ep->bl.weight_err = WBCGetTGCWeight( kp_wbc_ave_sample[ 0 ] \ + kp_wbc_ave_sample[ 1 ] \ + kp_wbc_ave_sample[ 2 ] \ + kp_wbc_ave_sample[ 3 ], &kp_wbc_tgc_weight, &( uwp->u.bl ) ) ; //----- Check the correction value if ( kp_wbc_tgc_weight < -0.5 ) { //----- Secure a bit of margin ep->bl.weight_err = KPAD_WBC_ERR_WRONG_ZERO ; } } } } //----- weight ave update for ( idx = 0 ; idx < WPAD_PRESS_UNITS ; idx ++ ) { ep->bl.weight_ave[ idx ] = kp_wbc_weight_ave[ idx ] ; } //----- tgc_weight update ep->bl.tgc_weight = kp_wbc_tgc_weight ; ep->bl.tgc_weight_err = kp_wbc_tgc_weight_issued ; } } } /******************************************************************************* READ KPAD (return the number of loaded buffers) *******************************************************************************/ s32 KPADRead( s32 chan, KPADStatus samplingBufs[], u32 length ) { return KPADiRead( chan, samplingBufs, length, NULL, FALSE ) ; } s32 KPADReadEx( s32 chan, KPADStatus samplingBufs[], u32 length, s32 *err ) { return KPADiRead( chan, samplingBufs, length, err, TRUE ) ; } static s32 KPADiRead( s32 chan, KPADStatus samplingBufs[], u32 length, s32 *err, BOOL keep ) { // This assumption is confirmed in the KPADInit function // (sizeof(KPADTmpStatus) == sizeof(KPADStatus)) KPADTmpStatus *tp = (KPADTmpStatus *)samplingBufs ; KPADInsideStatus *kp = &inside_kpads[ chan ] ; KPADStatus tmp ; KPADUnifiedWpadStatus *uwp ; s32 wpad_err ; s32 kpad_err = KPAD_READ_ERR_NONE ; s32 idx ; s32 copy_ct ; s32 return_ct = 0 ; u32 bufCount ; BOOL enabled ; u32 lastCoreButton ; u32 lastFsButton ; u32 lastClButton ; u32 lastDev ; BOOL corrupted ; BOOL changed ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; //----- Do not process if the KPADInit function has not been called if ( !kp_initialized ) { kpad_err = KPAD_READ_ERR_INIT ; goto read_finish ; } //----- Do not process until WPAD library initialization is complete if ( WPADGetStatus() != WPAD_STATE_SETUP ) { kpad_err = KPAD_READ_ERR_SETUP ; goto read_finish ; } //----- Do not process this if this was called from another thread mid-process enabled = OSDisableInterrupts() ; if ( kp->readLocked ) { (void)OSRestoreInterrupts( enabled ) ; kpad_err = KPAD_READ_ERR_LOCKED ; goto read_finish ; } kp->readLocked = TRUE ; wpad_err = WPADProbe( chan, NULL ) ; if ( wpad_err == WPAD_ERR_NO_CONTROLLER ) { //----- Reset internal parameters reset_kpad( kp ) ; if ( kp->dpd_ctrl_callback && kp->dpdPreCallbackDone && !kp->dpdPostCallbackDone ) { KPADiControlDpdCallback( chan, WPAD_ERR_NONE ) ; } //----- Remove locks kp->readLocked = FALSE ; (void)OSRestoreInterrupts( enabled ) ; kpad_err = KPAD_READ_ERR_NO_CONTROLLER ; goto read_finish ; } (void)OSRestoreInterrupts( enabled ) ; if ( kp->resetReq ) { // Data is invalid when a reset occurs kp->status.wpad_err = (s8)WPAD_ERR_INVALID ; reset_kpad( kp ) ; } //----- Copy the first element of the buffer that was passed in tmp = samplingBufs[ 0 ] ; if ( kp->bufCount > 1 && samplingBufs && length ) { enabled = OSDisableInterrupts() ; //----- Check the number of times that data has been saved to the buffer bufCount = kp->bufCount ; if ( bufCount > length ) { bufCount = length ; } copy_ct = (s32)bufCount ; //----- Flag initialization corrupted = FALSE ; changed = FALSE ; //----- Make the index match the most recent data idx = (s32) kp->bufIdx ; idx-- ; if ( idx < 0 ) { idx += kp->uniRingBufExLen + KPAD_RING_BUFS ; } //----- Get the most recent device if ( idx >= KPAD_RING_BUFS ) { uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ; } else { uwp = &kp->uniRingBuf[ idx ] ; } lastDev = uwp->u.core.dev ; do { // The data located immediately before the corrupted data may be also corrupt, even if the error code indicates there is no error, so we treat it as though it is corrupted. // if ( corrupted ) { if ( uwp->u.core.err == WPAD_ERR_NONE ) { uwp->u.core.err = WPAD_ERR_CORRUPTED ; corrupted = FALSE ; } else if ( uwp->u.core.err == WPAD_ERR_BUSY ) { corrupted = FALSE ; } } else if ( uwp->u.core.err == WPAD_ERR_CORRUPTED ) { corrupted = TRUE ; } if ( kp->status.dev_type != uwp->u.core.dev ) { changed = TRUE ; } if ( changed ) { uwp->u.core.err = WPAD_ERR_INVALID ; } //----- Advance the index by one idx-- ; if ( idx < 0 ) { idx += kp->uniRingBufExLen + KPAD_RING_BUFS ; } if ( idx >= KPAD_RING_BUFS ) { uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ; } else { uwp = &kp->uniRingBuf[ idx ] ; } } while ( --copy_ct > 0 ) ; //----- Leave the most recent data because the error check was not accurate kp->bufCount = 1 ; //----- Get the number of pieces of data in the buffer, excluding the most recent data if ( bufCount > 1 ) { copy_ct = (s32)( bufCount - 1 ) ; } else { copy_ct = 1 ; } //----- Number of pieces of data to return to the application return_ct = copy_ct ; //----- Save data to the sampling buffer, starting with the oldest tp += copy_ct ; //----- Index calculation idx = (s32)( kp->bufIdx - copy_ct - 1 ) ; if ( idx < 0 ) { idx += kp->uniRingBufExLen + KPAD_RING_BUFS ; } //----- Copy data to the buffer, starting with the oldest do { if ( idx >= KPAD_RING_BUFS ) { uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ; } else { uwp = &kp->uniRingBuf[ idx ] ; } --tp ; tp->w = *uwp ; idx++ ; if ( idx >= kp->uniRingBufExLen + KPAD_RING_BUFS ) { idx = 0 ; } } while ( --copy_ct > 0 ) ; (void)OSRestoreInterrupts( enabled ) ; // Obtain 1G value from the controller to absorb the individual differences between controllers. // Although we'd like to get the value in one try, the value cannot be retrieved immediately after connecting or attaching. // For the time being, it is obtained here each time through the loop. { WPADAcc core1G = {1, 1, 1} ; WPADAcc fs1G = {1, 1, 1} ; WPADGetAccGravityUnit( chan, WPAD_DEV_CORE, &core1G ) ; if(core1G.x * core1G.y * core1G.z != 0){ // Bug fix kp->acc_scale_x = 1.0f / core1G.x ; kp->acc_scale_y = 1.0f / core1G.y ; kp->acc_scale_z = 1.0f / core1G.z ; } else { // The values listed here are obtained with WPADGetAccGravityUnit(). // Accurate values should not be all that different. kp->acc_scale_x = 1.0f / 100 ; kp->acc_scale_y = 1.0f / 100 ; kp->acc_scale_z = 1.0f / 100 ; } WPADGetAccGravityUnit( chan, WPAD_DEV_FREESTYLE, &fs1G ) ; if( fs1G.x * fs1G.y * fs1G.z != 0 ){ // Bug fix kp->fs_acc_scale_x = 1.0f / fs1G.x ; kp->fs_acc_scale_y = 1.0f / fs1G.y ; kp->fs_acc_scale_z = 1.0f / fs1G.z ; } else { // The values listed here are obtained with WPADGetAccGravityUnit(). // Accurate values should not be all that different. kp->fs_acc_scale_x = 1.0f / 200 ; kp->fs_acc_scale_y = 1.0f / 200 ; kp->fs_acc_scale_z = 1.0f / 200 ; } } //----- Process obtained data, starting with the oldest copy_ct = return_ct ; tp = (KPADTmpStatus *)samplingBufs + copy_ct ; //----- Process one at a time if ( kp->btnProcMode == KPAD_BUTTON_PROC_MODE_TIGHT ) { do { --tp ; uwp = &tp->w ; //----- Check if the device type has changed if ( kp->status.dev_type != uwp->u.core.dev ) { kp->status.dev_type = uwp->u.core.dev ; // Update the device type kp->exResetReq = TRUE ; // Reset the extension controller information } //----- Copy the error code kp->status.wpad_err = uwp->u.core.err ; //----- Copy the data format kp->status.data_format = uwp->fmt ; //----- Get the device type when data was sent lastDev = kp->status.dev_type ; //----- Error processing for button input lastCoreButton = lastFsButton = lastClButton = KPAD_BUTTON_MASK ; switch ( uwp->u.core.err ) { case WPAD_ERR_NONE : if ( lastDev == WPAD_DEV_FREESTYLE ) { lastFsButton = uwp->u.fs.button ; lastClButton = 0 ; } else if ( lastDev == WPAD_DEV_CLASSIC || lastDev == WPAD_DEV_GUITAR || lastDev == WPAD_DEV_TRAIN ) { lastFsButton = 0 ; lastClButton = uwp->u.cl.clButton ; } else { lastFsButton = lastClButton = 0 ; } lastCoreButton = (u32)( uwp->u.core.button & 0x00009F1F ) ; // Enable Wii Remote buttons only break; case WPAD_ERR_CORRUPTED : lastFsButton = lastClButton = 0 ; // thru case WPAD_ERR_BUSY : lastCoreButton = (u32)( uwp->u.core.button & 0x00009F1F ) ; // Enable Wii Remote buttons only break ; default : // Do nothing and keep the previous state break ; } //----- If ACK is received from the Wii Remote, use the previous button information if ( lastCoreButton == KPAD_BUTTON_MASK ) { lastCoreButton = (u32)( kp->status.hold & 0x00009F1F ) ; } if ( lastFsButton == KPAD_BUTTON_MASK ) { lastFsButton = kp->status.hold ; } if ( lastClButton == KPAD_BUTTON_MASK ) { lastClButton = kp->status.ex_status.cl.hold ; } //----- Load button information read_kpad_button( kp, lastDev, 1, lastCoreButton, lastFsButton, lastClButton ) ; //----- Miscellaneous input processing switch ( uwp->u.core.err ) { case WPAD_ERR_NONE : read_kpad_ext( kp, uwp ) ; // thru case WPAD_ERR_CORRUPTED : read_kpad_acc( kp, uwp ) ; read_kpad_dpd( kp, uwp ) ; break ; default : if ( !keep ) { kp->status.dpd_valid_fg = 0 ; } break ; } tp[ 0 ].k = kp->status ; } while ( --copy_ct > 0 ) ; //----- As before, the most recent button presses are applied to all data in the buffer } else { //----- Find and get the most recent button and device type from the buffer lastDev = WPAD_DEV_NOT_FOUND ; lastCoreButton = lastFsButton = lastClButton = KPAD_BUTTON_MASK ; do { --tp ; uwp = &tp->w ; //----- Check if the device type has changed if ( kp->status.dev_type != uwp->u.core.dev ) { kp->status.dev_type = uwp->u.core.dev ; kp->exResetReq = TRUE ; } //----- Get the device type when data was sent lastDev = uwp->u.core.dev ; switch ( uwp->u.core.err ) { case WPAD_ERR_NONE : if ( lastDev == WPAD_DEV_FREESTYLE ) { lastFsButton = uwp->u.fs.button ; lastClButton = 0 ; } else if ( lastDev == WPAD_DEV_CLASSIC || lastDev == WPAD_DEV_GUITAR || lastDev == WPAD_DEV_TRAIN ) { lastFsButton = 0 ; lastClButton = uwp->u.cl.clButton ; } else { lastFsButton = lastClButton = 0 ; } // thru case WPAD_ERR_CORRUPTED : case WPAD_ERR_BUSY : lastCoreButton = (u32)( uwp->u.core.button & 0x00009F1F ) ; // Enable Wii Remote buttons only break ; default : break ; } } while ( --copy_ct > 0 ) ; //----- If there is no valid button data, inherit the previous data if ( lastCoreButton == KPAD_BUTTON_MASK ) { lastCoreButton = (u32)( kp->status.hold & 0x00009F1F ) ; } if ( lastFsButton == KPAD_BUTTON_MASK ) { lastFsButton = kp->status.hold ; } if ( lastClButton == KPAD_BUTTON_MASK ) { lastClButton = kp->status.ex_status.cl.hold ; } //----- Process button information read_kpad_button( kp, lastDev, (u32)return_ct, lastCoreButton, lastFsButton, lastClButton ) ; //----- Start other input processing once again from the start of the buffer copy_ct = return_ct ; tp = (KPADTmpStatus *)samplingBufs + copy_ct ; do { --tp ; uwp = &tp->w ; //----- Error code copy kp->status.wpad_err = uwp->u.core.err ; //----- Copy the data format kp->status.data_format = uwp->fmt ; //----- Miscellaneous input processing switch ( uwp->u.core.err ) { case WPAD_ERR_NONE : read_kpad_ext( kp, uwp ) ; // thru case WPAD_ERR_CORRUPTED : read_kpad_acc( kp, uwp ) ; read_kpad_dpd( kp, uwp ) ; break ; default : if ( !keep ) { kp->status.dpd_valid_fg = 0 ; } break ; } tp[ 0 ].k = kp->status ; } while ( --copy_ct > 0 ) ; } } //----- Remove locks kp->readLocked = FALSE ; read_finish: if ( return_ct == 0 ) { if ( kpad_err == KPAD_READ_ERR_NONE ) { //----- Even if there is no valid data, we will guarantee at least the final data if ( keep ) { samplingBufs[ 0 ] = tmp ; } kpad_err = KPAD_READ_ERR_NO_DATA ; } else if ( kpad_err == KPAD_READ_ERR_NO_CONTROLLER ) { if ( keep ) { samplingBufs[ 0 ].dev_type = WPAD_DEV_NOT_FOUND ; samplingBufs[ 0 ].data_format = WPAD_FMT_CORE ; samplingBufs[ 0 ].wpad_err = WPAD_ERR_NO_CONTROLLER ; } } } if ( err ) { *err = kpad_err ; } return return_ct ; } /******************************************************************************* INIT KPAD *******************************************************************************/ void KPADInit( void ) { KPADInitEx( NULL, 0 ); } void KPADInitEx( KPADUnifiedWpadStatus uniRingBufs[], u32 length ) { s32 i ; KPADInsideStatus *kp ; GXColor black = {0, 0, 0, 0} ; GXColor white = {255, 255, 255, 255} ; u32 idx ; // SDK 2.0 or later is required (see SDK release note 41) // Also KPADRead() assumes: (sizeof(KPADTmpStatus) == sizeof(KPADStatus)) if (offsetof( WPADFSStatus, err ) == offsetof( WPADStatus, err ) && sizeof(KPADTmpStatus) == sizeof(KPADStatus) ) { // OK } else { OSFatal( black, white, "KPADInit error" ) ; // Never reach here. } //----- Exit immediately if this is already initialized if ( kp_initialized ) { return ; } //----- WPAD WPADInit() ; //----- KPAD memset( &inside_kpads, 0, sizeof(inside_kpads) ) ; kp_err_dist_max = (f32)( 1.0f + (f32)WPADGetDpdSensitivity() ) ; i = 0 ; do { kp = &inside_kpads[ i ] ; //----- Configure the connection event callback kp->appConnectCallback = WPADSetConnectCallback( i, KPADiConnectCallback ) ; //----- Configure the sampling callback kp->appSamplingCallback = WPADSetSamplingCallback( i, KPADiSamplingCallback ) ; //----- The Wii Balance Board must be initialized kp_wbc_issued = FALSE ; kp_wbc_enabled = FALSE ; kp_wbc_setup = FALSE ; KPADResetWbcZeroPoint() ; //----- The DPD is enabled by default kp->dpdCurrState = FALSE ; kp->dpdNextState = TRUE ; kp->dpdCmd = WPADGetDpdFormat( i ) ; //----- Check device kp->status.dev_type = WPAD_DEV_NOT_FOUND ; kp->status.data_format = WPAD_FMT_CORE ; kp->dist_org = idist_org ; kp->accXY_nrm_hori = iaccXY_nrm_hori ; kp->sec_nrm_hori = isec_nrm_hori ; kp->center_org = icenter_org ; calc_dpd2pos_scale( kp ) ; //----- Initialize KPAD pointing behavior kp->pos_play_radius = kp->hori_play_radius = kp->dist_play_radius = kp->acc_play_radius = 0.0f ; kp->pos_sensitivity = kp->hori_sensitivity = kp->dist_sensitivity = kp->acc_sensitivity = 1.0f ; kp->pos_play_mode = kp->hori_play_mode = kp->dist_play_mode = kp->acc_play_mode = KPAD_PLAY_MODE_LOOSE ; //----- Initialize to no button repeat KPADSetBtnRepeat( i, 0.0f, 0.0f ) ; //----- Buttons are processed as before kp->btnProcMode = KPAD_BUTTON_PROC_MODE_LOOSE ; //----- Enabling error correction for the Sensor Bar placement position KPADEnableAimingMode( i ) ; //----- Nunchuk acceleration correction is disabled by default kp->fsAccRevise = 0 ; if ( length > 0 && uniRingBufs ) { ASSERT( ( length % 4 ) == 0 ) ; kp->uniRingBufExLen = length / 4 ; kp->uniRingBufEx = &uniRingBufs[ i * kp->uniRingBufExLen ] ; } else { kp->uniRingBufExLen = 0 ; kp->uniRingBufEx = NULL ; } for ( idx = 0 ; idx < KPAD_RING_BUFS ; idx++ ) { kp->uniRingBuf[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ; } for ( idx = 0 ; idx < kp->uniRingBufExLen ; idx++ ) { kp->uniRingBufEx[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ; } } while ( ++i < WPAD_MAX_CONTROLLERS ) ; //----- Cause a warning to be given when the WPAD library's callback registration function is called WPADSetCallbackByKPAD( TRUE ) ; //---- Initialize values KPADReset() ; //----- Rotation matrix initialization MTXRowCol( kp_fs_rot, 0, 0 ) = 1 ; MTXRowCol( kp_fs_rot, 0, 1 ) = 0 ; MTXRowCol( kp_fs_rot, 0, 2 ) = 0 ; MTXRowCol( kp_fs_rot, 0, 3 ) = 0 ; MTXRowCol( kp_fs_rot, 1, 0 ) = 0 ; MTXRowCol( kp_fs_rot, 1, 1 ) = (f32) cos( MTXDegToRad(kp_fs_revise_deg) ) ; MTXRowCol( kp_fs_rot, 1, 2 ) = (f32)-sin( MTXDegToRad(kp_fs_revise_deg) ) ; MTXRowCol( kp_fs_rot, 1, 3 ) = 0 ; MTXRowCol( kp_fs_rot, 2, 0 ) = 0 ; MTXRowCol( kp_fs_rot, 2, 1 ) = (f32) sin( MTXDegToRad(kp_fs_revise_deg) ) ; MTXRowCol( kp_fs_rot, 2, 2 ) = (f32) cos( MTXDegToRad(kp_fs_revise_deg) ) ; MTXRowCol( kp_fs_rot, 2, 3 ) = 0 ; kp_initialized = 1 ; OSRegisterVersion( __KPADVersion ) ; } /******************************************************************************* SHUTDOWN KPAD *******************************************************************************/ void KPADShutdown( void ) { KPADInsideStatus *kp ; s32 chan ; //----- Prevents a warning from being given, even if the WPAD library's callback registration function is called WPADSetCallbackByKPAD( FALSE ) ; for ( chan = 0 ; chan < WPAD_MAX_CONTROLLERS ; chan++ ) { kp = &inside_kpads[ chan ] ; if ( kp->appSamplingCallback ) { WPADSetSamplingCallback( chan, kp->appSamplingCallback ) ; } else { WPADSetSamplingCallback( chan, NULL ) ; } if ( kp->appConnectCallback ) { WPADSetConnectCallback( chan, kp->appConnectCallback ) ; } else { WPADSetConnectCallback( chan, NULL ) ; } } //----- The KPADInit function must be called to reuse KPAD kp_initialized = 0 ; } /******************************************************************************* RESET KPAD *******************************************************************************/ void KPADReset( void ) { s32 chan ; //----- Recalculate constants set_obj_interval( kp_obj_interval ) ; //---- Reset all KPADs chan = WPAD_MAX_CONTROLLERS - 1 ; do { if ( WPADGetStatus() == WPAD_STATE_SETUP ) { WPADStopMotor( chan ) ; } inside_kpads[ chan ].resetReq = TRUE ; } while ( --chan >= 0 ) ; } /******************************************************************************* Processing Immediately Following a Connection *******************************************************************************/ static void KPADiConnectCallback( s32 chan, s32 reason ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; u32 idx ; if ( reason == WPAD_ERR_NONE ) { //----- We are going to set the sampling callback, so temporarily prevent warnings from being given WPADSetCallbackByKPAD( FALSE ) ; //----- Configure the sampling callback WPADSetSamplingCallback( chan, KPADiSamplingCallback ) ; //----- Sampling callback is set, so re-enable warnings WPADSetCallbackByKPAD( TRUE ) ; //----- Reset the DPD state kp->dpdCurrState = FALSE ; kp->dpdCmd = WPAD_DPD_OFF ; //----- 4P might be for the Wii Balance Board, so initialize it. if ( chan == WPAD_CHAN3 ) { kp_wbc_issued = FALSE ; kp_wbc_enabled = FALSE ; kp_wbc_setup = FALSE ; KPADResetWbcZeroPoint() ; } } else { //----- Throw away any remaining data in the ring buffer for ( idx = 0 ; idx < KPAD_RING_BUFS ; idx++ ) { kp->uniRingBuf[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ; } for ( idx = 0 ; idx < kp->uniRingBufExLen ; idx++ ) { kp->uniRingBufEx[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ; } } if ( kp->appConnectCallback ) { kp->appConnectCallback( chan, reason ) ; } } WPADConnectCallback KPADSetConnectCallback( s32 chan, WPADConnectCallback callback ) { WPADConnectCallback prevCallback ; BOOL enable ; enable = OSDisableInterrupts() ; prevCallback = inside_kpads[ chan ].appConnectCallback ; inside_kpads[ chan ].appConnectCallback = callback ; OSRestoreInterrupts( enable ) ; return prevCallback ; } /******************************************************************************* Wii Balance Board Processing *******************************************************************************/ static void KPADiControlWbcCallback( s32 chan, s32 result ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; ASSERT( chan == WPAD_CHAN3 ) ; kp_wbc_enabled = (u8)( ( result == WPAD_ERR_NONE ) ? TRUE : FALSE ) ; kp_wbc_issued = FALSE ; } static void KPADiUpdateTempWbcCallback( s32 chan, s32 result ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; ASSERT( chan == WPAD_CHAN3 ) ; kp_wbc_zero_point_done = (u8)( ( result == WPAD_ERR_NONE ) ? 1 : 0 ) ; kp_wbc_issued = FALSE ; } void KPADResetWbcZeroPoint( void ) { KPADInsideStatus *kp = &inside_kpads[ WPAD_CHAN3 ] ; int idx ; kp_wbc_zero_point_done = 0 ; kp_wbc_ave_sample_count = 0 ; kp_wbc_tgc_weight_issued = 0 ; for ( idx = 0 ; idx < WPAD_PRESS_UNITS ; idx ++ ) { kp_wbc_ave_sample[ idx ] = 0 ; kp_wbc_weight_ave[ idx ] = 0 ; } } void KPADResetWbcTgcWeight( void ) { KPADInsideStatus *kp = &inside_kpads[ WPAD_CHAN3 ] ; kp_wbc_ave_sample_count = 0 ; kp_wbc_tgc_weight_issued = 1 ; kp_wbc_ave_sample[ 0 ] = kp_wbc_ave_sample[ 1 ] = kp_wbc_ave_sample[ 2 ] = kp_wbc_ave_sample[ 3 ] = 0 ; } /******************************************************************************* DPD Control Functions *******************************************************************************/ void KPADDisableDPD( const s32 chan ) { inside_kpads[ chan ].dpdNextState = FALSE ; } void KPADEnableDPD ( const s32 chan ) { inside_kpads[ chan ].dpdNextState = TRUE ; } void KPADSetControlDpdCallback( s32 chan, KPADControlDpdCallback callback ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; BOOL enabled ; enabled = OSDisableInterrupts() ; kp->dpd_ctrl_callback = callback ; (void)OSRestoreInterrupts( enabled ) ; } static void KPADiControlDpdCallback( s32 chan, s32 result ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; if ( result == WPAD_ERR_NONE ) { if ( kp->dpd_ctrl_callback && !kp->dpdPostCallbackDone ) { kp->dpdPostCallbackDone = TRUE ; kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_FINISHED ) ; kp->dpdPreCallbackDone = FALSE ; } } kp->dpdCurrState = (u8)WPADIsDpdEnabled( chan ) ; kp->dpdIssued = FALSE ; } /******************************************************************************* Processing when data is obtained *******************************************************************************/ static void KPADiSamplingCallback(s32 chan) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; KPADUnifiedWpadStatus *uwp ; u32 idx ; u32 type ; u32 curDpd ; f32 aimClbr ; static struct { u8 dpd ; u8 fmt ; } table[] = { { WPAD_DPD_OFF, WPAD_FMT_CORE_ACC }, { WPAD_DPD_EXP, WPAD_FMT_CORE_ACC_DPD }, { WPAD_DPD_OFF, WPAD_FMT_FREESTYLE_ACC }, { WPAD_DPD_STD, WPAD_FMT_FREESTYLE_ACC_DPD }, { WPAD_DPD_OFF, WPAD_FMT_CLASSIC_ACC }, { WPAD_DPD_STD, WPAD_FMT_CLASSIC_ACC_DPD }, { WPAD_DPD_OFF, WPAD_FMT_GUITAR }, { WPAD_DPD_STD, WPAD_FMT_GUITAR }, { WPAD_DPD_OFF, WPAD_FMT_BALANCE_CHECKER }, { WPAD_DPD_OFF, WPAD_FMT_BALANCE_CHECKER }, { WPAD_DPD_OFF, WPAD_FMT_TRAIN }, { WPAD_DPD_OFF, WPAD_FMT_TRAIN } } ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ); if ( WPADProbe( chan, &type ) == WPAD_ERR_NO_CONTROLLER ) { goto finish ; } idx = kp->bufIdx ; if ( idx >= KPAD_RING_BUFS + kp->uniRingBufExLen ) { idx = 0 ; } if ( idx >= KPAD_RING_BUFS ) { uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ; } else { uwp = &kp->uniRingBuf[ idx ] ; } WPADRead( chan, &uwp->u ) ; uwp->fmt = (u8)WPADGetDataFormat( chan ) ; kp->bufIdx = (u8)( idx + 1 ) ; if ( kp->bufCount < kp->uniRingBufExLen + KPAD_RING_BUFS ) { kp->bufCount++ ; } if ( kp->aimReq ) { if ( kp->aimEnabled ) { if ( WPAD_SENSOR_BAR_POS_TOP == WPADGetSensorBarPosition() ) { aimClbr = 0.2f ; } else { aimClbr = -0.2f ; } } else { aimClbr = 0.0f ; } KPADSetSensorHeight( chan, aimClbr ) ; kp->aimReq = FALSE ; } //----- Check DPD settings switch ( type ) { case WPAD_DEV_CORE : case WPAD_DEV_FUTURE : case WPAD_DEV_NOT_SUPPORTED : case WPAD_DEV_UNKNOWN : idx = 0 ; break ; case WPAD_DEV_FREESTYLE : idx = 2 ; break ; case WPAD_DEV_CLASSIC : idx = 4 ; break ; case WPAD_DEV_GUITAR : idx = 6 ; break ; case WPAD_DEV_BALANCE_CHECKER : idx = 8 ; break ; case WPAD_DEV_TRAIN : idx = 10 ; break ; default : goto finish ; break ; } if ( kp->dpdNextState ) { idx += 1 ; } curDpd = (u32)( WPADIsDpdEnabled( chan ) ? kp->dpdCmd : WPAD_DPD_OFF ); if ( curDpd != table[idx].dpd ) { if ( kp->dpd_ctrl_callback && !kp->dpdPreCallbackDone ) { kp->dpdPreCallbackDone = TRUE ; kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_START ) ; kp->dpdPostCallbackDone = FALSE ; } if ( !kp->dpdIssued ) { kp->dpdIssued = TRUE ; if (WPADControlDpd( chan, table[ idx ].dpd, KPADiControlDpdCallback ) == WPAD_ERR_NONE ) { kp->dpdCmd = table[ idx ].dpd ; } } } else { if ( uwp->fmt != table[ idx ].fmt ) { WPADSetDataFormat( chan, table[ idx ].fmt ); } } //----- Initialize data required to measure weight on the Wii Balance Board if ( type == WPAD_DEV_BALANCE_CHECKER ) { if ( !kp_wbc_issued && !kp_wbc_enabled ) { //----- Start the Wii Balance Board if ( WPAD_ERR_NONE == WPADControlBLC( chan, WPAD_BLCMD_ON, KPADiControlWbcCallback ) ) { kp_wbc_issued = TRUE ; } } else if ( kp_wbc_enabled && !kp_wbc_setup ) { //----- Get the calibration values if ( !kp_wbc_issued ) { if ( WPAD_ERR_NONE == WBCSetupCalibration() ) { kp_wbc_issued = TRUE ; } } else { kp_wbc_setup = (u8)( WBCGetCalibrationStatus() ) ; kp_wbc_issued = (u8)( ( kp_wbc_setup ) ? FALSE : TRUE ) ; } } else if ( !kp_wbc_issued && kp_wbc_setup && kp_wbc_zero_point_done < 3 ) { switch ( kp_wbc_zero_point_done ) { case 0 : //----- Update the temperature if ( WPAD_ERR_NONE == WPADControlBLC( chan, WPAD_BLCMD_UPDATE_TEMP, KPADiUpdateTempWbcCallback ) ) { kp_wbc_issued = TRUE ; } break ; case 1 : //----- Wait until the temperature has stabilized if ( uwp->u.bl.temp == 127 || uwp->u.bl.temp == -128 ) { KPADResetWbcZeroPoint() ; } else if ( ++kp_wbc_ave_sample_count > kp_wbc_wait_count ) { kp_wbc_zero_point_done = 2 ; kp_wbc_ave_sample_count = 0 ; } break ; case 2 : //----- Get the zero-point correction values if ( uwp->u.bl.err == WPAD_ERR_NONE ) { //----- Calculate the average value over two seconds kp_wbc_ave_sample_count++ ; for ( idx = 0 ; idx < WPAD_PRESS_UNITS ; idx ++ ) { //----- For tgc_weight kp_wbc_ave_sample[ idx ] = ( kp_wbc_ave_sample[ idx ] * ( kp_wbc_ave_sample_count - 1 ) + uwp->u.bl.press[ idx ] ) / kp_wbc_ave_sample_count ; } if ( kp_wbc_ave_sample_count > kp_wbc_ave_count ) { kp_wbc_zero_point_done = 3 ; WBCSetZEROPoint( kp_wbc_ave_sample, WPAD_PRESS_UNITS ) ; } } break ; } } } finish : if ( kp->appSamplingCallback ) { kp->appSamplingCallback( chan ) ; } } WPADSamplingCallback KPADSetSamplingCallback( s32 chan, WPADSamplingCallback callback ) { WPADSamplingCallback prevCallback ; BOOL enable ; enable = OSDisableInterrupts() ; prevCallback = inside_kpads[ chan ].appSamplingCallback ; inside_kpads[ chan ].appSamplingCallback = callback ; OSRestoreInterrupts( enable ) ; return prevCallback ; } /******************************************************************************* Get WPAD-Format Data *******************************************************************************/ void KPADGetUnifiedWpadStatus( s32 chan, KPADUnifiedWpadStatus *dst, u32 count ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; KPADUnifiedWpadStatus *uwp ; BOOL enabled ; u32 idx ; ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; if ( count > kp->uniRingBufExLen + KPAD_RING_BUFS ) { count = kp->uniRingBufExLen + KPAD_RING_BUFS ; } enabled = OSDisableInterrupts() ; idx = kp->bufIdx ; while ( count-- ) { if ( idx == 0 ) { idx = kp->uniRingBufExLen + KPAD_RING_BUFS - 1 ; } else { idx-- ; } // <== latest ... oldest ==> // dst[0] dst[1] ... dst[KPAD_RING_BUFS - 1] if ( idx >= KPAD_RING_BUFS ) { uwp = &kp->uniRingBufEx[ idx - KPAD_RING_BUFS ] ; } else { uwp = &kp->uniRingBuf[ idx ] ; } if ( WPADGetStatus() != WPAD_STATE_SETUP ) { uwp->u.core.err = WPAD_ERR_INVALID ; } memcpy( dst, uwp, sizeof(KPADUnifiedWpadStatus) ) ; dst++ ; } (void)OSRestoreInterrupts( enabled ) ; } /******************************************************************************* Clamping Method for Control Stick *******************************************************************************/ void KPADEnableStickCrossClamp( void ) { kp_stick_clamp_cross = TRUE ; } void KPADDisableStickCrossClamp( void ) { kp_stick_clamp_cross = FALSE ; } /******************************************************************************* Angle Correction for Nunchuk Acceleration *******************************************************************************/ void KPADSetReviseMode( s32 chan, BOOL sw ) { KPADInsideStatus *kp = &inside_kpads[ chan ] ; kp->fsAccRevise = (u8)sw ; } f32 KPADReviseAcc( Vec *acc ) { MTXMultVec( kp_fs_rot, acc, acc ) ; return kp_fs_revise_deg ; // Return the correction angle (in degrees). } f32 KPADGetReviseAngle( void ) { return kp_fs_revise_deg ; // Return the correction angle (in degrees). } /******************************************************************************* Controls for All Calculation Methods *******************************************************************************/ void KPADSetPosPlayMode( s32 chan, KPADPlayMode mode ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].pos_play_mode = mode ; } void KPADSetHoriPlayMode( s32 chan, KPADPlayMode mode ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].hori_play_mode = mode ; } void KPADSetDistPlayMode( s32 chan, KPADPlayMode mode ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].dist_play_mode = mode ; } void KPADSetAccPlayMode( s32 chan, KPADPlayMode mode ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].acc_play_mode = mode ; } KPADPlayMode KPADGetPosPlayMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; return ( inside_kpads[ chan ].pos_play_mode ) ; } KPADPlayMode KPADGetHoriPlayMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; return ( inside_kpads[ chan ].hori_play_mode ) ; } KPADPlayMode KPADGetDistPlayMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; return ( inside_kpads[ chan ].dist_play_mode ) ; } KPADPlayMode KPADGetAccPlayMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; return ( inside_kpads[ chan ].acc_play_mode ) ; } /******************************************************************************* Button Processing Method Controls *******************************************************************************/ void KPADSetButtonProcMode( s32 chan, u8 mode ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; inside_kpads[ chan ].btnProcMode = mode ; } u8 KPADGetButtonProcMode( s32 chan ) { ASSERT( (0 <= chan) && (chan < WPAD_MAX_CONTROLLERS) ) ; return ( inside_kpads[ chan ].btnProcMode ) ; }