Lines Matching refs:kp
133 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in get_ring_buffer_by_kpad1_style() local
161 idx1 = kp->bufIdx - 1 ; in get_ring_buffer_by_kpad1_style()
171 switch( kp->uniRingBuf[idx1].u.core.dev ) { in get_ring_buffer_by_kpad1_style()
195 kp->uniRingBuf[idx1].u.core.err = WPAD_ERR_INVALID ; in get_ring_buffer_by_kpad1_style()
198 &kp->uniRingBuf[ idx1 ].u, in get_ring_buffer_by_kpad1_style()
216 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in KPADSetBtnRepeat() local
221 kp->btn_repeat_delay = (u16)(s32)( delay_sec * 200.0f + 0.5f ) ; in KPADSetBtnRepeat()
222 kp->btn_repeat_pulse = (u16)(s32)( pulse_sec * 200.0f + 0.5f ) ; in KPADSetBtnRepeat()
225 kp->btn_repeat_delay = KPAD_BTN_NO_RPT_DELAY ; in KPADSetBtnRepeat()
226 kp->btn_repeat_pulse = 0 ; in KPADSetBtnRepeat()
230 kp->btn_repeat_time = 0 ; in KPADSetBtnRepeat()
231 kp->btn_repeat_next = kp->btn_repeat_delay ; in KPADSetBtnRepeat()
232 kp->btn_cl_repeat_time = 0 ; in KPADSetBtnRepeat()
233 kp->btn_cl_repeat_next = kp->btn_repeat_delay ; in KPADSetBtnRepeat()
322 static void calc_dpd2pos_scale( KPADInsideStatus *kp ) in calc_dpd2pos_scale() argument
335 if ( kp->center_org.x < 0.0f ) { in calc_dpd2pos_scale()
336 sx += kp->center_org.x ; in calc_dpd2pos_scale()
338 sx -= kp->center_org.x ; in calc_dpd2pos_scale()
342 if ( kp->center_org.y < 0.0f ) { in calc_dpd2pos_scale()
343 sy += kp->center_org.y ; in calc_dpd2pos_scale()
345 sy -= kp->center_org.y ; in calc_dpd2pos_scale()
349 kp->dpd2pos_scale = scale / ( ( sx < sy ) ? sx : sy ) ; in calc_dpd2pos_scale()
356 static void reset_kpad( KPADInsideStatus *kp ) in reset_kpad() argument
359 KPADStatus *sp = &kp->status ; in reset_kpad()
360 KPADEXStatus *ep = &kp->status.ex_status ; in reset_kpad()
362 kp->resetReq = FALSE ; in reset_kpad()
365 kp->kobj_frame_min.x = -1.0f + kp_err_outside_frame ; in reset_kpad()
366 kp->kobj_frame_max.x = 1.0f - kp_err_outside_frame ; in reset_kpad()
367 kp->kobj_frame_min.y = -((f32)KPAD_DPD_RESO_WY / KPAD_DPD_RESO_WX) + kp_err_outside_frame ; in reset_kpad()
368 kp->kobj_frame_max.y = ((f32)KPAD_DPD_RESO_WY / KPAD_DPD_RESO_WX) - kp_err_outside_frame ; in reset_kpad()
370 kp->err_dist_speed_1 = 1.0f / kp_err_dist_speed ; in reset_kpad()
371 kp->err_dist_speedM_1 = -1.0f / kp_err_dist_speed ; in reset_kpad()
372 kp->ah_circle_radius2 = kp_ah_circle_radius * kp_ah_circle_radius ; in reset_kpad()
374 kp->err_dist_min = kp_err_dist_min ; in reset_kpad()
375 kp->dist_vv1 = kp_dist_vv1 ; in reset_kpad()
379 kp->btn_repeat_time = 0 ; in reset_kpad()
380 kp->btn_repeat_next = kp->btn_repeat_delay ; in reset_kpad()
384 kp->dpd_valid2_ct = 0 ; in reset_kpad()
389 sp->horizon.x = kp->acc_horizon.x = kp->obj_horizon.x = 1.0f ; in reset_kpad()
390 sp->horizon.y = kp->acc_horizon.y = kp->obj_horizon.y = 0.0f ; in reset_kpad()
397 sp->dist = kp->dist_org ; in reset_kpad()
400 kp->sec_dist = sp->dist ; in reset_kpad()
401 kp->sec_length = kp->trust_sec_length = kp->dist_vv1 / kp->sec_dist ; in reset_kpad()
402 kp->sec_nrm = kp->sec_nrm_hori ; in reset_kpad()
409 kp->hard_acc = sp->acc ; in reset_kpad()
411 kp->ah_circle_pos = kp->acc_horizon ; in reset_kpad()
412 kp->ah_circle_ct = kp_ah_circle_ct ; in reset_kpad()
415 kp->valid_objs = 0 ; in reset_kpad()
417 op = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; in reset_kpad()
420 } while ( --op >= kp->kobj_sample ) ; in reset_kpad()
422 op = &kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ] ; in reset_kpad()
425 } while ( --op >= kp->kobj_regular ) ; in reset_kpad()
428 kp->bufCount = 0 ; // Ring buffer unprocessed in reset_kpad()
431 kp->exResetReq = TRUE ; in reset_kpad()
455 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in KPADCalibrateDPD() local
456 KPADStatus *sp = &kp->status ; in KPADCalibrateDPD()
462 if ( kp->valid_objs != KPAD_USE_OBJECTS ) return ( kp->valid_objs ) ; in KPADCalibrateDPD()
467 vx = kp->hard_acc.x ; in KPADCalibrateDPD()
468 vy = kp->hard_acc.y ; in KPADCalibrateDPD()
471 kp->accXY_nrm_hori.x = vx / f1 ; in KPADCalibrateDPD()
472 kp->accXY_nrm_hori.y = vy / f1 ; in KPADCalibrateDPD()
478 op1 = kp->kobj_sample ; in KPADCalibrateDPD()
488 kp->kobj_regular[ 0 ] = *op1 ; in KPADCalibrateDPD()
489 kp->kobj_regular[ 1 ] = *op2 ; in KPADCalibrateDPD()
493 kp->kobj_regular[ 0 ] = *op2 ; in KPADCalibrateDPD()
494 kp->kobj_regular[ 1 ] = *op1 ; in KPADCalibrateDPD()
501 calc_dpd2pos_scale( kp ) ; in KPADCalibrateDPD()
504 vx = kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ].center.x - kp->kobj_regular[ 0 ].center.x ; in KPADCalibrateDPD()
505 vy = kp->kobj_regular[ KPAD_USE_OBJECTS - 1 ].center.y - kp->kobj_regular[ 0 ].center.y ; in KPADCalibrateDPD()
507 kp->sec_nrm_hori.x = vx * f1 ; in KPADCalibrateDPD()
508 kp->sec_nrm_hori.y = vy * f1 ; in KPADCalibrateDPD()
513 kp->dist_org = kp->dist_vv1 * f1 ; in KPADCalibrateDPD()
520 return ( kp->valid_objs ) ; in KPADCalibrateDPD()
551 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in KPADSetSensorHeight() local
553 kp->center_org.x = 0.0f ; in KPADSetSensorHeight()
554 kp->center_org.y = -level ; in KPADSetSensorHeight()
555 calc_dpd2pos_scale( kp ) ; in KPADSetSensorHeight()
562 static void calc_button_repeat( KPADInsideStatus *kp, u32 dev_type, u32 count ) in calc_button_repeat() argument
564 KPADStatus *sp = &kp->status ; in calc_button_repeat()
565 KPADEXStatus *ep = &kp->status.ex_status ; in calc_button_repeat()
569 kp->btn_repeat_time = 0 ; in calc_button_repeat()
570 kp->btn_repeat_next = kp->btn_repeat_delay ; in calc_button_repeat()
573 if ( sp->trig && kp->btn_repeat_pulse ) { in calc_button_repeat()
578 kp->btn_repeat_time += count ; in calc_button_repeat()
579 if ( kp->btn_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) { in calc_button_repeat()
580 kp->btn_repeat_time -= KPAD_BTN_NO_RPT_DELAY ; in calc_button_repeat()
584 if ( kp->btn_repeat_time >= kp->btn_repeat_next ) { in calc_button_repeat()
588 kp->btn_repeat_next += kp->btn_repeat_pulse ; in calc_button_repeat()
591 if ( kp->btn_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) { in calc_button_repeat()
592 kp->btn_repeat_time -= KPAD_BTN_RPT_TIME_MAX ; in calc_button_repeat()
593 kp->btn_repeat_next -= KPAD_BTN_RPT_TIME_MAX ; in calc_button_repeat()
601 kp->btn_cl_repeat_time = 0 ; in calc_button_repeat()
602 kp->btn_cl_repeat_next = kp->btn_repeat_delay ; in calc_button_repeat()
605 if ( ep->cl.trig && kp->btn_repeat_pulse ) { in calc_button_repeat()
610 kp->btn_cl_repeat_time += count ; in calc_button_repeat()
611 if ( kp->btn_cl_repeat_time >= KPAD_BTN_NO_RPT_DELAY ) { in calc_button_repeat()
612 kp->btn_cl_repeat_time -= KPAD_BTN_NO_RPT_DELAY ; in calc_button_repeat()
616 if ( kp->btn_cl_repeat_time >= kp->btn_cl_repeat_next ) { in calc_button_repeat()
620 kp->btn_cl_repeat_next += kp->btn_repeat_pulse ; in calc_button_repeat()
623 if ( kp->btn_cl_repeat_time >= KPAD_BTN_RPT_TIME_MAX ) { in calc_button_repeat()
624 kp->btn_cl_repeat_time -= KPAD_BTN_RPT_TIME_MAX ; in calc_button_repeat()
625 kp->btn_cl_repeat_next -= KPAD_BTN_RPT_TIME_MAX ; in calc_button_repeat()
636 static void read_kpad_button( KPADInsideStatus *kp, u32 dev_type, u32 count, u32 core, u32 fs, u32 … in read_kpad_button() argument
638 KPADStatus *sp = &kp->status ; in read_kpad_button()
639 KPADEXStatus *ep = &kp->status.ex_status ; in read_kpad_button()
669 calc_button_repeat( kp, dev_type, count ) ; in read_kpad_button()
676 static void calc_acc( KPADInsideStatus *kp, f32 *acc, f32 acc2 ) in calc_acc() argument
684 if ( kp->acc_play_mode == KPAD_PLAY_MODE_LOOSE ) { in calc_acc()
692 if ( f1 >= kp->acc_play_radius ) { in calc_acc()
697 f1 /= kp->acc_play_radius ; in calc_acc()
701 f1 *= kp->acc_sensitivity ; in calc_acc()
706 if ( f2 < -kp->acc_play_radius ) { in calc_acc()
707 *acc += ( f2 + kp->acc_play_radius ) * kp->acc_sensitivity ; in calc_acc()
708 } else if ( f2 > kp->acc_play_radius ) { in calc_acc()
709 *acc += ( f2 - kp->acc_play_radius ) * kp->acc_sensitivity ; in calc_acc()
718 static void calc_acc_horizon( KPADInsideStatus *kp ) in calc_acc_horizon() argument
724 f1 = sqrtf( kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ; in calc_acc_horizon()
726 ax = kp->hard_acc.x / f1 ; in calc_acc_horizon()
727 ay = kp->hard_acc.y / f1 ; in calc_acc_horizon()
736 vx = kp->accXY_nrm_hori.x * ax + kp->accXY_nrm_hori.y * ay ; in calc_acc_horizon()
737 vy = kp->accXY_nrm_hori.y * ax - kp->accXY_nrm_hori.x * ay ; in calc_acc_horizon()
740 ax = ( vx - kp->acc_horizon.x ) * f1 + kp->acc_horizon.x ; in calc_acc_horizon()
741 ay = ( vy - kp->acc_horizon.y ) * f1 + kp->acc_horizon.y ; in calc_acc_horizon()
746 kp->acc_horizon.x = ax / f1 ; in calc_acc_horizon()
747 kp->acc_horizon.y = ay / f1 ; in calc_acc_horizon()
751 kp->ah_circle_pos.x += ( kp->acc_horizon.x - kp->ah_circle_pos.x ) * kp_ah_circle_pw ; in calc_acc_horizon()
752 kp->ah_circle_pos.y += ( kp->acc_horizon.y - kp->ah_circle_pos.y ) * kp_ah_circle_pw ; in calc_acc_horizon()
754 vx = kp->acc_horizon.x - kp->ah_circle_pos.x ; in calc_acc_horizon()
755 vy = kp->acc_horizon.y - kp->ah_circle_pos.y ; in calc_acc_horizon()
756 if ( vx*vx + vy*vy <= kp->ah_circle_radius2 ) { in calc_acc_horizon()
757 if ( kp->ah_circle_ct ) -- kp->ah_circle_ct ; in calc_acc_horizon()
759 kp->ah_circle_ct = kp_ah_circle_ct ; in calc_acc_horizon()
763 static void calc_acc_vertical( KPADInsideStatus *kp ) in calc_acc_vertical() argument
765 KPADStatus *sp = &kp->status ; in calc_acc_vertical()
770 ax = sqrtf( f2 = kp->hard_acc.x * kp->hard_acc.x + kp->hard_acc.y * kp->hard_acc.y ) ; in calc_acc_vertical()
771 ay = - kp->hard_acc.z ; in calc_acc_vertical()
808 static void read_kpad_acc( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp ) in read_kpad_acc() argument
810 KPADStatus *sp = &kp->status ; in read_kpad_acc()
829 kp->hard_acc.x = clamp_acc( (f32)(s32)-uwp->u.core.accX * kp->acc_scale_x, kp_rm_acc_max ) ; in read_kpad_acc()
830 kp->hard_acc.y = clamp_acc( (f32)(s32)-uwp->u.core.accZ * kp->acc_scale_z, kp_rm_acc_max ) ; in read_kpad_acc()
831 kp->hard_acc.z = clamp_acc( (f32)(s32) uwp->u.core.accY * kp->acc_scale_y, kp_rm_acc_max ) ; in read_kpad_acc()
837 calc_acc( kp, &sp->acc.x, kp->hard_acc.x ) ; in read_kpad_acc()
838 calc_acc( kp, &sp->acc.y, kp->hard_acc.y ) ; in read_kpad_acc()
839 calc_acc( kp, &sp->acc.z, kp->hard_acc.z ) ; in read_kpad_acc()
849 calc_acc_horizon( kp ) ; in read_kpad_acc()
850 calc_acc_vertical( kp ) ; in read_kpad_acc()
863 fsrc.x = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccX * kp->fs_acc_scale_x, kp_fs_acc_max ) ; in read_kpad_acc()
864 fsrc.y = clamp_acc( (f32)(s32)-uwp->u.fs.fsAccZ * kp->fs_acc_scale_z, kp_fs_acc_max ) ; in read_kpad_acc()
865 fsrc.z = clamp_acc( (f32)(s32) uwp->u.fs.fsAccY * kp->fs_acc_scale_y, kp_fs_acc_max ) ; in read_kpad_acc()
867 if ( kp->fsAccRevise ) { in read_kpad_acc()
875 calc_acc( kp, &sp->ex_status.fs.acc.x, fsrc.x ) ; in read_kpad_acc()
876 calc_acc( kp, &sp->ex_status.fs.acc.y, fsrc.y ) ; in read_kpad_acc()
877 calc_acc( kp, &sp->ex_status.fs.acc.z, fsrc.z ) ; in read_kpad_acc()
891 static void get_kobj( KPADInsideStatus *kp, DPDObject *wobj_p ) in get_kobj() argument
900 kobj_p = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; in get_kobj()
915 } while ( --kobj_p >= kp->kobj_sample ) ; in get_kobj()
922 static void check_kobj_outside_frame( KPADInsideStatus *kp, KPADObject *kobj_t ) in check_kobj_outside_frame() argument
929 if ( kobj_p->center.x <= kp->kobj_frame_min.x || kobj_p->center.x >= kp->kobj_frame_max.x || in check_kobj_outside_frame()
930 … kobj_p->center.y <= kp->kobj_frame_min.y || kobj_p->center.y >= kp->kobj_frame_max.y ) { in check_kobj_outside_frame()
964 static f32 calc_horizon( KPADInsideStatus *kp, Vec2 *p1, Vec2 *p2, Vec2 *hori ) in calc_horizon() argument
975 hori->x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ; in calc_horizon()
976 hori->y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ; in calc_horizon()
978 return ( kp->dist_vv1 * f1 ) ; in calc_horizon()
985 static s8 select_2obj_first( KPADInsideStatus *kp ) in select_2obj_first() argument
991 op1 = kp->kobj_sample ; in select_2obj_first()
999 f1 = calc_horizon( kp, &op1->center, &op2->center, &hori ) ; in select_2obj_first()
1002 if ( f1 <= kp->err_dist_min || f1 >= kp_err_dist_max ) continue ; in select_2obj_first()
1004 f1 = kp->acc_horizon.x * hori.x + kp->acc_horizon.y * hori.y ; in select_2obj_first()
1019 } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; in select_2obj_first()
1020 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; in select_2obj_first()
1025 kp->kobj_regular[ 0 ] = *rp1 ; in select_2obj_first()
1026 kp->kobj_regular[ 1 ] = *rp2 ; in select_2obj_first()
1035 static s8 select_2obj_continue( KPADInsideStatus *kp ) in select_2obj_continue() argument
1044 op1 = kp->kobj_sample ; in select_2obj_continue()
1060 f1 *= kp->dist_vv1 ; // Distance in select_2obj_continue()
1061 if ( f1 <= kp->err_dist_min || f1 >= kp_err_dist_max ) continue ; in select_2obj_continue()
1064 f1 -= kp->sec_dist ; in select_2obj_continue()
1066 f1 *= kp->err_dist_speedM_1 ; in select_2obj_continue()
1068 f1 *= kp->err_dist_speed_1 ; in select_2obj_continue()
1073 f2 = kp->sec_nrm.x * nrm.x + kp->sec_nrm.y * nrm.y ; in select_2obj_continue()
1096 } while ( ++op2 <= &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; in select_2obj_continue()
1097 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; in select_2obj_continue()
1102 kp->kobj_regular[ 0 ] = *rp1 ; in select_2obj_continue()
1103 kp->kobj_regular[ 1 ] = *rp2 ; in select_2obj_continue()
1112 static s8 select_1obj_first( KPADInsideStatus *kp ) in select_1obj_first() argument
1120 vx = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ; in select_1obj_first()
1121 vy = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ; in select_1obj_first()
1124 vx *= kp->trust_sec_length ; in select_1obj_first()
1125 vy *= kp->trust_sec_length ; in select_1obj_first()
1128 op1 = kp->kobj_sample ; in select_1obj_first()
1137 if ( p1.x <= kp->kobj_frame_min.x || p1.x >= kp->kobj_frame_max.x || in select_1obj_first()
1138 p1.y <= kp->kobj_frame_min.y || p1.y >= kp->kobj_frame_max.y ) { in select_1obj_first()
1140 if ( p2.x > kp->kobj_frame_min.x && p2.x < kp->kobj_frame_max.x && in select_1obj_first()
1141 p2.y > kp->kobj_frame_min.y && p2.y < kp->kobj_frame_max.y ) { in select_1obj_first()
1143 kp->kobj_regular[ 1 ] = *op1 ; in select_1obj_first()
1145 kp->kobj_regular[ 0 ].center = p1 ; in select_1obj_first()
1146 kp->kobj_regular[ 0 ].error_fg = 0 ; in select_1obj_first()
1147 kp->kobj_regular[ 0 ].state_fg = -1 ; in select_1obj_first()
1153 if ( p2.x <= kp->kobj_frame_min.x || p2.x >= kp->kobj_frame_max.x || in select_1obj_first()
1154 p2.y <= kp->kobj_frame_min.y || p2.y >= kp->kobj_frame_max.y ) { in select_1obj_first()
1156 kp->kobj_regular[ 0 ] = *op1 ; in select_1obj_first()
1158 kp->kobj_regular[ 1 ].center = p2 ; in select_1obj_first()
1159 kp->kobj_regular[ 1 ].error_fg = 0 ; in select_1obj_first()
1160 kp->kobj_regular[ 1 ].state_fg = -1 ; in select_1obj_first()
1166 } while ( ++op1 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ; in select_1obj_first()
1175 static s8 select_1obj_continue( KPADInsideStatus *kp ) in select_1obj_continue() argument
1183 op1 = kp->kobj_regular ; in select_1obj_continue()
1188 op2 = kp->kobj_sample ; in select_1obj_continue()
1200 } while ( ++op2 < &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS ] ) ; in select_1obj_continue()
1201 } while ( ++op1 < &kp->kobj_regular[ KPAD_USE_OBJECTS ] ) ; in select_1obj_continue()
1209 … kp->sec_nrm.x = kp->sec_nrm_hori.x * kp->acc_horizon.x + kp->sec_nrm_hori.y * kp->acc_horizon.y ; in select_1obj_continue()
1210 … kp->sec_nrm.y = kp->sec_nrm_hori.y * kp->acc_horizon.x - kp->sec_nrm_hori.x * kp->acc_horizon.y ; in select_1obj_continue()
1213 vx = kp->sec_length * kp->sec_nrm.x ; in select_1obj_continue()
1214 vy = kp->sec_length * kp->sec_nrm.y ; in select_1obj_continue()
1215 if ( rp1 == &kp->kobj_regular[ 0 ] ) { in select_1obj_continue()
1216 kp->kobj_regular[ 1 ].center.x = rp1->center.x + vx ; in select_1obj_continue()
1217 kp->kobj_regular[ 1 ].center.y = rp1->center.y + vy ; in select_1obj_continue()
1218 kp->kobj_regular[ 1 ].error_fg = 0 ; in select_1obj_continue()
1219 kp->kobj_regular[ 1 ].state_fg = -1 ; in select_1obj_continue()
1221 kp->kobj_regular[ 0 ].center.x = rp1->center.x - vx ; in select_1obj_continue()
1222 kp->kobj_regular[ 0 ].center.y = rp1->center.y - vy ; in select_1obj_continue()
1223 kp->kobj_regular[ 0 ].error_fg = 0 ; in select_1obj_continue()
1224 kp->kobj_regular[ 0 ].state_fg = -1 ; in select_1obj_continue()
1227 if ( kp->status.dpd_valid_fg < 0 ) { in select_1obj_continue()
1238 static void calc_obj_horizon( KPADInsideStatus *kp ) in calc_obj_horizon() argument
1243 vx = kp->kobj_regular[ 1 ].center.x - kp->kobj_regular[ 0 ].center.x ; in calc_obj_horizon()
1244 vy = kp->kobj_regular[ 1 ].center.y - kp->kobj_regular[ 0 ].center.y ; in calc_obj_horizon()
1245 kp->sec_length = sqrtf( vx * vx + vy * vy ) ; // Should not be zero in calc_obj_horizon()
1247 f1 = 1.0f / kp->sec_length ; in calc_obj_horizon()
1248 kp->sec_dist = kp->dist_vv1 * f1 ; in calc_obj_horizon()
1250 kp->sec_nrm.x = ( vx *= f1 ) ; in calc_obj_horizon()
1251 kp->sec_nrm.y = ( vy *= f1 ) ; in calc_obj_horizon()
1253 kp->obj_horizon.x = kp->sec_nrm_hori.x * vx + kp->sec_nrm_hori.y * vy ; in calc_obj_horizon()
1254 kp->obj_horizon.y = kp->sec_nrm_hori.y * vx - kp->sec_nrm_hori.x * vy ; in calc_obj_horizon()
1261 static void calc_dpd_variable( KPADInsideStatus *kp, s8 valid_fg_next ) in calc_dpd_variable() argument
1263 KPADStatus *sp = &kp->status ; in calc_dpd_variable()
1277 pos.x = kp->sec_nrm_hori.x * kp->sec_nrm.x + kp->sec_nrm_hori.y * kp->sec_nrm.y ; in calc_dpd_variable()
1278 pos.y = kp->sec_nrm_hori.y * kp->sec_nrm.x - kp->sec_nrm_hori.x * kp->sec_nrm.y ; in calc_dpd_variable()
1292 if ( kp->hori_play_mode == KPAD_PLAY_MODE_LOOSE ) { in calc_dpd_variable()
1294 if ( f1 >= kp->hori_play_radius ) { in calc_dpd_variable()
1299 f1 /= kp->hori_play_radius ; in calc_dpd_variable()
1303 f1 *= kp->hori_sensitivity ; in calc_dpd_variable()
1318 if ( f1 > kp->hori_play_radius ) { in calc_dpd_variable()
1320 f1 = ( f1 - kp->hori_play_radius ) / f1 * kp->hori_sensitivity ; in calc_dpd_variable()
1344 dist = kp->dist_vv1 / kp->sec_length ; in calc_dpd_variable()
1361 if ( kp->dist_play_mode == KPAD_PLAY_MODE_LOOSE ) { in calc_dpd_variable()
1363 if ( f1 >= kp->dist_play_radius ) { in calc_dpd_variable()
1368 f1 /= kp->dist_play_radius ; in calc_dpd_variable()
1372 f1 *= kp->dist_sensitivity ; in calc_dpd_variable()
1384 if ( f1 > kp->dist_play_radius ) { in calc_dpd_variable()
1386 f1 = ( f1 - kp->dist_play_radius ) / f1 * kp->dist_sensitivity ; in calc_dpd_variable()
1407 pos.x = ( kp->kobj_regular[ 0 ].center.x + kp->kobj_regular[ 1 ].center.x ) * 0.5f ; in calc_dpd_variable()
1408 pos.y = ( kp->kobj_regular[ 0 ].center.y + kp->kobj_regular[ 1 ].center.y ) * 0.5f ; in calc_dpd_variable()
1411 f1 = kp->sec_nrm.x * kp->sec_nrm_hori.x + kp->sec_nrm.y * kp->sec_nrm_hori.y ; in calc_dpd_variable()
1412 f2 = -kp->sec_nrm.y * kp->sec_nrm_hori.x + kp->sec_nrm.x * kp->sec_nrm_hori.y ; in calc_dpd_variable()
1417 vec.x = ( kp->center_org.x - vec.x ) * kp->dpd2pos_scale ; in calc_dpd_variable()
1418 vec.y = ( kp->center_org.y - vec.y ) * kp->dpd2pos_scale ; in calc_dpd_variable()
1421 pos.x = -kp->accXY_nrm_hori.y * vec.x + kp->accXY_nrm_hori.x * vec.y ; in calc_dpd_variable()
1422 pos.y = -kp->accXY_nrm_hori.x * vec.x - kp->accXY_nrm_hori.y * vec.y ; in calc_dpd_variable()
1436 if ( kp->pos_play_mode == KPAD_PLAY_MODE_LOOSE ) { in calc_dpd_variable()
1438 if ( f1 >= kp->pos_play_radius ) { in calc_dpd_variable()
1443 f1 /= kp->pos_play_radius ; in calc_dpd_variable()
1447 f1 *= kp->pos_sensitivity ; in calc_dpd_variable()
1457 if ( f1 > kp->pos_play_radius ) { in calc_dpd_variable()
1459 f1 = ( f1 - kp->pos_play_radius ) / f1 * kp->pos_sensitivity ; in calc_dpd_variable()
1484 static void read_kpad_dpd( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp ) in read_kpad_dpd() argument
1486 KPADStatus *sp = &kp->status ; in read_kpad_dpd()
1497 get_kobj( kp, &uwp->u.core.obj[ WPAD_DPD_MAX_OBJECTS - 1 ] ) ; in read_kpad_dpd()
1500 op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; in read_kpad_dpd()
1503 } while ( --op1 >= kp->kobj_sample ) ; in read_kpad_dpd()
1510 check_kobj_outside_frame( kp, kp->kobj_sample ) ; // Set surrounding objects to invalid in read_kpad_dpd()
1511 …check_kobj_same_position( kp->kobj_sample ) ; // Set objects at the same coordinate to be invalid in read_kpad_dpd()
1514 kp->valid_objs = 0 ; in read_kpad_dpd()
1515 op1 = &kp->kobj_sample[ WPAD_DPD_MAX_OBJECTS - 1 ] ; in read_kpad_dpd()
1517 if ( op1->error_fg == 0 ) ++ kp->valid_objs ; in read_kpad_dpd()
1518 } while ( --op1 >= kp->kobj_sample ) ; in read_kpad_dpd()
1525 if ( kp->valid_objs >= 2 ) in read_kpad_dpd()
1527 valid_fg_next = select_2obj_continue( kp ) ; in read_kpad_dpd()
1530 if ( kp->valid_objs >= 1 ) in read_kpad_dpd()
1532 valid_fg_next = select_1obj_continue( kp ) ; in read_kpad_dpd()
1537 if ( kp->valid_objs >= 2 ) in read_kpad_dpd()
1539 valid_fg_next = select_2obj_first( kp ) ; in read_kpad_dpd()
1542 if ( kp->valid_objs >= 1 ) in read_kpad_dpd()
1544 valid_fg_next = select_1obj_continue( kp ) ; in read_kpad_dpd()
1549 if ( kp->valid_objs >= 2 ) in read_kpad_dpd()
1551 valid_fg_next = select_2obj_first( kp ) ; in read_kpad_dpd()
1555 if ( kp->valid_objs == 1 ) in read_kpad_dpd()
1557 valid_fg_next = select_1obj_first( kp ) ; in read_kpad_dpd()
1571 calc_obj_horizon( kp ) ; in read_kpad_dpd()
1574 if ( kp->ah_circle_ct == 0 ) { in read_kpad_dpd()
1575 …if ( kp->obj_horizon.x * kp->acc_horizon.x + kp->obj_horizon.y * kp->acc_horizon.y <= kp_err_acc_i… in read_kpad_dpd()
1578 kp->kobj_regular[ 0 ].error_fg = in read_kpad_dpd()
1579 kp->kobj_regular[ 1 ].error_fg = 1 ; in read_kpad_dpd()
1585 if ( kp->dpd_valid2_ct == 200 ) { in read_kpad_dpd()
1586 kp->trust_sec_length = kp->sec_length ; in read_kpad_dpd()
1588 ++ kp->dpd_valid2_ct ; in read_kpad_dpd()
1591 kp->dpd_valid2_ct = 0 ; in read_kpad_dpd()
1594 kp->dpd_valid2_ct = 0 ; in read_kpad_dpd()
1598 calc_dpd_variable( kp, valid_fg_next ) ; in read_kpad_dpd()
1676 static void read_kpad_stick( KPADInsideStatus *kp, KPADUnifiedWpadStatus *uwp ) in read_kpad_stick() argument
1678 KPADEXStatus *ep = &kp->status.ex_status ; in read_kpad_stick()
1688 if ( kp->exResetReq ) { in read_kpad_stick()
1689 kp->exResetReq = FALSE ; in read_kpad_stick()
1706 if ( kp->exResetReq ) { in read_kpad_stick()
1707 kp->exResetReq = FALSE ; in read_kpad_stick()
1714 kp->btn_cl_repeat_time = 0 ; in read_kpad_stick()
1715 kp->btn_cl_repeat_next = kp->btn_repeat_delay ; in read_kpad_stick()
1736 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in KPADRead() local
1756 if ( kp->readLocked ) { in KPADRead()
1760 kp->readLocked = TRUE ; in KPADRead()
1764 kp->dpd_ctrl_callback && in KPADRead()
1765 kp->dpdPreCallbackDone && in KPADRead()
1766 !kp->dpdPostCallbackDone ) { in KPADRead()
1772 if ( kp->resetReq ) { in KPADRead()
1773 kp->status.wpad_err = (s8)wpad_err ; in KPADRead()
1774 reset_kpad( kp ) ; in KPADRead()
1782 if ( kp->bufCount && in KPADRead()
1787 bufCount = kp->bufCount ; in KPADRead()
1794 kp->bufCount = 0 ; in KPADRead()
1799 idx = (s32)( kp->bufIdx - copy_ct ) ; in KPADRead()
1807 tp->w = kp->uniRingBuf[ idx ] ; in KPADRead()
1813 uwStatus = kp->uniRingBuf[ idx ] ; in KPADRead()
1827 kp->acc_scale_x = 1.0f / core1G.x ; in KPADRead()
1828 kp->acc_scale_y = 1.0f / core1G.y ; in KPADRead()
1829 kp->acc_scale_z = 1.0f / core1G.z ; in KPADRead()
1833 kp->acc_scale_x = 1.0f / 100 ; in KPADRead()
1834 kp->acc_scale_y = 1.0f / 100 ; in KPADRead()
1835 kp->acc_scale_z = 1.0f / 100 ; in KPADRead()
1840 kp->fs_acc_scale_x = 1.0f / fs1G.x ; in KPADRead()
1841 kp->fs_acc_scale_y = 1.0f / fs1G.y ; in KPADRead()
1842 kp->fs_acc_scale_z = 1.0f / fs1G.z ; in KPADRead()
1846 kp->fs_acc_scale_x = 1.0f / 200 ; in KPADRead()
1847 kp->fs_acc_scale_y = 1.0f / 200 ; in KPADRead()
1848 kp->fs_acc_scale_z = 1.0f / 200 ; in KPADRead()
1890 memcpy( samplingBufs, &kp->status, sizeof(KPADStatus) ) ; in KPADRead()
1896 lastFsButton = kp->status.hold ; in KPADRead()
1899 lastClButton = kp->status.ex_status.cl.hold ; in KPADRead()
1901 read_kpad_button( kp, lastDev, bufCount, lastCoreButton, lastFsButton, lastClButton ) ; in KPADRead()
1912 kp->status.wpad_err = uwp->u.core.err ; in KPADRead()
1913 if ( kp->status.dev_type != uwp->u.core.dev ) { in KPADRead()
1917 kp->status.dev_type = uwp->u.core.dev ; in KPADRead()
1918 kp->exResetReq = TRUE ; in KPADRead()
1921 kp->status.data_format = uwp->fmt ; in KPADRead()
1925 read_kpad_stick( kp, uwp ) ; in KPADRead()
1928 read_kpad_acc( kp, uwp ) ; in KPADRead()
1929 read_kpad_dpd( kp, uwp ) ; in KPADRead()
1933 kp->status.dpd_valid_fg = FALSE ; in KPADRead()
1936 tp[ 1 ].k = kp->status ; in KPADRead()
1942 kp->readLocked = FALSE ; in KPADRead()
1955 KPADInsideStatus *kp ; in KPADInit() local
1979 kp = &inside_kpads[ i ] ; in KPADInit()
1982 kp->dpdEnabled = TRUE ; in KPADInit()
1983 kp->dpdCmd = WPAD_DPD_OFF ; in KPADInit()
1986 kp->status.dev_type = WPAD_DEV_NOT_FOUND ; in KPADInit()
1987 kp->status.data_format = WPAD_FMT_CORE ; in KPADInit()
1989 kp->dist_org = idist_org ; in KPADInit()
1990 kp->accXY_nrm_hori = iaccXY_nrm_hori ; in KPADInit()
1991 kp->sec_nrm_hori = isec_nrm_hori ; in KPADInit()
1992 kp->center_org = icenter_org ; in KPADInit()
1993 calc_dpd2pos_scale( kp ) ; in KPADInit()
1996 kp->pos_play_radius = in KPADInit()
1997 kp->hori_play_radius = in KPADInit()
1998 kp->dist_play_radius = in KPADInit()
1999 kp->acc_play_radius = 0.0f ; in KPADInit()
2001 kp->pos_sensitivity = in KPADInit()
2002 kp->hori_sensitivity = in KPADInit()
2003 kp->dist_sensitivity = in KPADInit()
2004 kp->acc_sensitivity = 1.0f ; in KPADInit()
2006 kp->pos_play_mode = in KPADInit()
2007 kp->hori_play_mode = in KPADInit()
2008 kp->dist_play_mode = in KPADInit()
2009 kp->acc_play_mode = KPAD_PLAY_MODE_LOOSE ; in KPADInit()
2018 kp->fsAccRevise = 0 ; in KPADInit()
2036 kp->uniRingBuf[idx].u.core.err = WPAD_ERR_NO_CONTROLLER ; in KPADInit()
2079 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in KPADSetControlDpdCallback() local
2083 kp->dpd_ctrl_callback = callback ; in KPADSetControlDpdCallback()
2089 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in KPADiSamplingCallback() local
2113 idx = kp->bufIdx ; in KPADiSamplingCallback()
2118 uwp = &kp->uniRingBuf[ idx ] ; in KPADiSamplingCallback()
2122 kp->bufIdx = (u8)( idx + 1 ) ; in KPADiSamplingCallback()
2123 if ( kp->bufCount < KPAD_RING_BUFS ) { in KPADiSamplingCallback()
2124 kp->bufCount++ ; in KPADiSamplingCallback()
2127 if ( kp->aimReq ) { in KPADiSamplingCallback()
2128 if ( kp->aimEnabled ) { in KPADiSamplingCallback()
2139 kp->aimReq = FALSE ; in KPADiSamplingCallback()
2163 if ( kp->dpdEnabled ) { in KPADiSamplingCallback()
2167 curDpd = (u32)( WPADIsDpdEnabled( chan ) ? kp->dpdCmd : WPAD_DPD_OFF ); in KPADiSamplingCallback()
2170 if ( kp->dpd_ctrl_callback && in KPADiSamplingCallback()
2171 !kp->dpdPreCallbackDone ) { in KPADiSamplingCallback()
2172 kp->dpdPreCallbackDone = TRUE ; in KPADiSamplingCallback()
2173 kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_START ) ; in KPADiSamplingCallback()
2174 kp->dpdPostCallbackDone = FALSE ; in KPADiSamplingCallback()
2176 if ( !kp->dpdIssued ) { in KPADiSamplingCallback()
2177 kp->dpdIssued = TRUE ; in KPADiSamplingCallback()
2180 kp->dpdCmd = table[ idx ].dpd ; in KPADiSamplingCallback()
2190 if ( kp->appSamplingCallback ) { in KPADiSamplingCallback()
2191 kp->appSamplingCallback( chan ) ; in KPADiSamplingCallback()
2197 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in KPADiControlDpdCallback() local
2200 if ( kp->dpd_ctrl_callback && in KPADiControlDpdCallback()
2201 !kp->dpdPostCallbackDone ) { in KPADiControlDpdCallback()
2202 kp->dpdPostCallbackDone = TRUE ; in KPADiControlDpdCallback()
2203 kp->dpd_ctrl_callback( chan, KPAD_STATE_CTRL_DPD_FINISHED ) ; in KPADiControlDpdCallback()
2204 kp->dpdPreCallbackDone = FALSE ; in KPADiControlDpdCallback()
2207 kp->dpdIssued = FALSE ; in KPADiControlDpdCallback()
2212 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in KPADGetUnifiedWpadStatus() local
2224 idx = kp->bufIdx ; in KPADGetUnifiedWpadStatus()
2234 kp->uniRingBuf[ idx ].u.core.err = WPAD_ERR_INVALID ; in KPADGetUnifiedWpadStatus()
2236 memcpy( dst, &kp->uniRingBuf[idx], sizeof(KPADUnifiedWpadStatus) ) ; in KPADGetUnifiedWpadStatus()
2260 KPADInsideStatus *kp = &inside_kpads[ chan ] ; in KPADSetReviseMode() local
2262 kp->fsAccRevise = (u8)sw ; in KPADSetReviseMode()