1 /* This source as presented is a modified version of original wiiuse for use
2 * with RetroArch, and must not be confused with the original software. */
3
4 #include <stdio.h>
5 #include <math.h>
6 #include <time.h>
7
8 #ifndef WIN32
9 #include <unistd.h>
10 #endif
11 #ifdef GEKKO
12 #include <ogcsys.h>
13 #endif
14 #include "definitions.h"
15 #include "wiiuse_internal.h"
16 #include "ir.h"
17
18 static int ir_correct_for_bounds(float* x, float* y, enum aspect_t aspect, int offset_x, int offset_y);
19 static void ir_convert_to_vres(float* x, float* y, enum aspect_t aspect, unsigned int vx, unsigned int vy);
20
21 /**
22 * @brief Get the IR sensitivity settings.
23 *
24 * @param wm Pointer to a wiimote_t structure.
25 * @param block1 [out] Pointer to where block1 will be set.
26 * @param block2 [out] Pointer to where block2 will be set.
27 *
28 * @return Returns the sensitivity level.
29 */
get_ir_sens(struct wiimote_t * wm,char ** block1,char ** block2)30 static int get_ir_sens(struct wiimote_t* wm, char** block1, char** block2) {
31 if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL1)) {
32 *block1 = WM_IR_BLOCK1_LEVEL1;
33 *block2 = WM_IR_BLOCK2_LEVEL1;
34 return 1;
35 } else if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL2)) {
36 *block1 = WM_IR_BLOCK1_LEVEL2;
37 *block2 = WM_IR_BLOCK2_LEVEL2;
38 return 2;
39 } else if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL3)) {
40 *block1 = WM_IR_BLOCK1_LEVEL3;
41 *block2 = WM_IR_BLOCK2_LEVEL3;
42 return 3;
43 } else if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL4)) {
44 *block1 = WM_IR_BLOCK1_LEVEL4;
45 *block2 = WM_IR_BLOCK2_LEVEL4;
46 return 4;
47 } else if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR_SENS_LVL5)) {
48 *block1 = WM_IR_BLOCK1_LEVEL5;
49 *block2 = WM_IR_BLOCK2_LEVEL5;
50 return 5;
51 }
52
53 *block1 = NULL;
54 *block2 = NULL;
55 return 0;
56 }
57
rotate_dots(struct fdot_t * in,struct fdot_t * out,int count,float ang)58 static void rotate_dots(struct fdot_t* in, struct fdot_t *out, int count, float ang) {
59 float s, c;
60 int i;
61
62 if (ang == 0) {
63 for (i = 0; i < count; ++i) {
64 out[i].x = in[i].x;
65 out[i].y = in[i].y;
66 }
67 return;
68 }
69
70 s = sin(DEGREE_TO_RAD(ang));
71 c = cos(DEGREE_TO_RAD(ang));
72
73 /*
74 * [ cos(theta) -sin(theta) ][ ir->rx ]
75 * [ sin(theta) cos(theta) ][ ir->ry ]
76 */
77
78 for (i = 0; i < count; ++i) {
79 out[i].x = (c * in[i].x) + (-s * in[i].y);
80 out[i].y = (s * in[i].x) + (c * in[i].y);
81 }
82 }
83
84 /**
85 * @brief Correct for the IR bounding box.
86 *
87 * @param x [out] The current X, it will be updated if valid.
88 * @param y [out] The current Y, it will be updated if valid.
89 * @param aspect Aspect ratio of the screen.
90 * @param offset_x The X offset of the bounding box.
91 * @param offset_y The Y offset of the bounding box.
92 *
93 * @return Returns 1 if the point is valid and was updated.
94 *
95 * Nintendo was smart with this bit. They sacrifice a little
96 * precision for a big increase in usability.
97 */
ir_correct_for_bounds(float * x,float * y,enum aspect_t aspect,int offset_x,int offset_y)98 static int ir_correct_for_bounds(float* x, float* y, enum aspect_t aspect, int offset_x, int offset_y) {
99 float x0, y0;
100 int xs, ys;
101
102 if (aspect == WIIUSE_ASPECT_16_9) {
103 xs = WM_ASPECT_16_9_X;
104 ys = WM_ASPECT_16_9_Y;
105 } else {
106 xs = WM_ASPECT_4_3_X;
107 ys = WM_ASPECT_4_3_Y;
108 }
109
110 x0 = ((1024 - xs) / 2) + offset_x;
111 y0 = ((768 - ys) / 2) + offset_y;
112
113 if ((*x >= x0)
114 && (*x <= (x0 + xs))
115 && (*y >= y0)
116 && (*y <= (y0 + ys)))
117 {
118 *x -= offset_x;
119 *y -= offset_y;
120
121 return 1;
122 }
123
124 return 0;
125 }
126
127 /**
128 * @brief Interpolate the point to the user defined virtual screen resolution.
129 */
ir_convert_to_vres(float * x,float * y,enum aspect_t aspect,unsigned int vx,unsigned int vy)130 static void ir_convert_to_vres(float* x, float* y, enum aspect_t aspect, unsigned int vx, unsigned int vy) {
131 int xs, ys;
132
133 if (aspect == WIIUSE_ASPECT_16_9) {
134 xs = WM_ASPECT_16_9_X;
135 ys = WM_ASPECT_16_9_Y;
136 } else {
137 xs = WM_ASPECT_4_3_X;
138 ys = WM_ASPECT_4_3_Y;
139 }
140
141 *x -= ((1024-xs)/2);
142 *y -= ((768-ys)/2);
143
144 *x = (*x / (float)xs) * vx;
145 *y = (*y / (float)ys) * vy;
146 }
147
wiiuse_set_ir_mode(struct wiimote_t * wm)148 void wiiuse_set_ir_mode(struct wiimote_t *wm)
149 {
150 ubyte buf = 0x00;
151
152 if(!wm) return;
153 if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_IR)) return;
154
155 if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP)) buf = WM_IR_TYPE_BASIC;
156 else buf = WM_IR_TYPE_EXTENDED;
157 wiiuse_write_data(wm,WM_REG_IR_MODENUM, &buf, 1, NULL);
158 }
159
wiiuse_set_ir(struct wiimote_t * wm,int status)160 void wiiuse_set_ir(struct wiimote_t *wm,int status)
161 {
162 ubyte buf = 0x00;
163 int ir_level = 0;
164 char* block1 = NULL;
165 char* block2 = NULL;
166
167 if(!wm) return;
168
169 /*
170 * Wait for the handshake to finish first.
171 * When it handshake finishes and sees that
172 * IR is enabled, it will call this function
173 * again to actually enable IR.
174 */
175 if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_HANDSHAKE_COMPLETE)) {
176 WIIUSE_DEBUG("Tried to enable IR, will wait until handshake finishes.\n");
177 if(status)
178 WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_INIT);
179 else
180 WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR_INIT);
181 return;
182 }
183
184 /*
185 * Check to make sure a sensitivity setting is selected.
186 */
187 ir_level = get_ir_sens(wm, &block1, &block2);
188 if (!ir_level) {
189 WIIUSE_ERROR("No IR sensitivity setting selected.");
190 return;
191 }
192
193 if (status) {
194 /* if already enabled then stop */
195 if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
196 wiiuse_status(wm,NULL);
197 return;
198 }
199 } else {
200 /* if already disabled then stop */
201 if (!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) {
202 wiiuse_status(wm,NULL);
203 return;
204 }
205 }
206
207 buf = (status ? 0x04 : 0x00);
208 wiiuse_sendcmd(wm,WM_CMD_IR,&buf,1,NULL);
209 wiiuse_sendcmd(wm,WM_CMD_IR_2,&buf,1,NULL);
210
211 if (!status) {
212 WIIUSE_DEBUG("Disabled IR cameras for wiimote id %i.", wm->unid);
213 wiiuse_status(wm,NULL);
214 return;
215 }
216
217 /* enable IR, set sensitivity */
218 buf = 0x08;
219 wiiuse_write_data(wm,WM_REG_IR,&buf,1,NULL);
220
221 wiiuse_write_data(wm, WM_REG_IR_BLOCK1, (ubyte*)block1, 9, NULL);
222 wiiuse_write_data(wm, WM_REG_IR_BLOCK2, (ubyte*)block2, 2, NULL);
223
224 if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP)) buf = WM_IR_TYPE_BASIC;
225 else buf = WM_IR_TYPE_EXTENDED;
226 wiiuse_write_data(wm,WM_REG_IR_MODENUM, &buf, 1, NULL);
227
228 wiiuse_status(wm,NULL);
229 return;
230 }
231
232 /**
233 * @brief Set the virtual screen resolution for IR tracking.
234 *
235 * @param wm Pointer to a wiimote_t structure.
236 * @param status 1 to enable, 0 to disable.
237 */
wiiuse_set_ir_vres(struct wiimote_t * wm,unsigned int x,unsigned int y)238 void wiiuse_set_ir_vres(struct wiimote_t* wm, unsigned int x, unsigned int y) {
239 if (!wm) return;
240
241 wm->ir.vres[0] = (x-1);
242 wm->ir.vres[1] = (y-1);
243 }
244
245 /**
246 * @brief Set the XY position for the IR cursor.
247 *
248 * @param wm Pointer to a wiimote_t structure.
249 */
wiiuse_set_ir_position(struct wiimote_t * wm,enum ir_position_t pos)250 void wiiuse_set_ir_position(struct wiimote_t* wm, enum ir_position_t pos) {
251 if (!wm) return;
252
253 wm->ir.pos = pos;
254
255 switch (pos) {
256
257 case WIIUSE_IR_ABOVE:
258 wm->ir.offset[0] = 0;
259
260 if (wm->ir.aspect == WIIUSE_ASPECT_16_9)
261 wm->ir.offset[1] = WM_ASPECT_16_9_Y/2 - 70;
262 else if (wm->ir.aspect == WIIUSE_ASPECT_4_3)
263 wm->ir.offset[1] = WM_ASPECT_4_3_Y/2 - 100;
264
265 return;
266
267 case WIIUSE_IR_BELOW:
268 wm->ir.offset[0] = 0;
269
270 if (wm->ir.aspect == WIIUSE_ASPECT_16_9)
271 wm->ir.offset[1] = -WM_ASPECT_16_9_Y/2 + 70;
272 else if (wm->ir.aspect == WIIUSE_ASPECT_4_3)
273 wm->ir.offset[1] = -WM_ASPECT_4_3_Y/2 + 100;
274
275 return;
276
277 default:
278 return;
279 };
280 }
281
282 /**
283 * @brief Set the aspect ratio of the TV/monitor.
284 *
285 * @param wm Pointer to a wiimote_t structure.
286 * @param aspect Either WIIUSE_ASPECT_16_9 or WIIUSE_ASPECT_4_3
287 */
wiiuse_set_aspect_ratio(struct wiimote_t * wm,enum aspect_t aspect)288 void wiiuse_set_aspect_ratio(struct wiimote_t* wm, enum aspect_t aspect) {
289 if (!wm) return;
290
291 wm->ir.aspect = aspect;
292
293 if (aspect == WIIUSE_ASPECT_4_3) {
294 wm->ir.vres[0] = WM_ASPECT_4_3_X;
295 wm->ir.vres[1] = WM_ASPECT_4_3_Y;
296 } else {
297 wm->ir.vres[0] = WM_ASPECT_16_9_X;
298 wm->ir.vres[1] = WM_ASPECT_16_9_Y;
299 }
300
301 /* reset the position offsets */
302 wiiuse_set_ir_position(wm, wm->ir.pos);
303 }
304
305 /**
306 * @brief Set the IR sensitivity.
307 *
308 * @param wm Pointer to a wiimote_t structure.
309 * @param level 1-5, same as Wii system sensitivity setting.
310 *
311 * If the level is < 1, then level will be set to 1.
312 * If the level is > 5, then level will be set to 5.
313 */
wiiuse_set_ir_sensitivity(struct wiimote_t * wm,int level)314 void wiiuse_set_ir_sensitivity(struct wiimote_t* wm, int level) {
315 char* block1 = NULL;
316 char* block2 = NULL;
317
318 if (!wm) return;
319
320 if (level > 5) level = 5;
321 if (level < 1) level = 1;
322
323 WIIMOTE_DISABLE_STATE(wm, (WIIMOTE_STATE_IR_SENS_LVL1 |
324 WIIMOTE_STATE_IR_SENS_LVL2 |
325 WIIMOTE_STATE_IR_SENS_LVL3 |
326 WIIMOTE_STATE_IR_SENS_LVL4 |
327 WIIMOTE_STATE_IR_SENS_LVL5));
328
329 switch (level) {
330 case 1:
331 WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL1);
332 break;
333 case 2:
334 WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL2);
335 break;
336 case 3:
337 WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL3);
338 break;
339 case 4:
340 WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL4);
341 break;
342 case 5:
343 WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_IR_SENS_LVL5);
344 break;
345 default:
346 return;
347 }
348
349 if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_IR)) return;
350
351 /* set the new sensitivity */
352 get_ir_sens(wm, &block1, &block2);
353
354 wiiuse_write_data(wm, WM_REG_IR_BLOCK1, (ubyte*)block1, 9,NULL);
355 wiiuse_write_data(wm, WM_REG_IR_BLOCK2, (ubyte*)block2, 2,NULL);
356
357 WIIUSE_DEBUG("Set IR sensitivity to level %i (unid %i)", level, wm->unid);
358 }
359
360 /**
361 * @brief Calculate the data from the IR spots. Basic IR mode.
362 *
363 * @param wm Pointer to a wiimote_t structure.
364 * @param data Data returned by the wiimote for the IR spots.
365 */
calculate_basic_ir(struct wiimote_t * wm,ubyte * data)366 void calculate_basic_ir(struct wiimote_t* wm, ubyte* data) {
367 struct ir_dot_t* dot = wm->ir.dot;
368 int i;
369
370 dot[0].rx = 1023 - (data[0] | ((data[2] & 0x30) << 4));
371 dot[0].ry = data[1] | ((data[2] & 0xC0) << 2);
372
373 dot[1].rx = 1023 - (data[3] | ((data[2] & 0x03) << 8));
374 dot[1].ry = data[4] | ((data[2] & 0x0C) << 6);
375
376 dot[2].rx = 1023 - (data[5] | ((data[7] & 0x30) << 4));
377 dot[2].ry = data[6] | ((data[7] & 0xC0) << 2);
378
379 dot[3].rx = 1023 - (data[8] | ((data[7] & 0x03) << 8));
380 dot[3].ry = data[9] | ((data[7] & 0x0C) << 6);
381
382 /* set each IR spot to visible if spot is in range */
383 for (i = 0; i < 4; ++i) {
384 dot[i].rx = BIG_ENDIAN_SHORT(dot[i].rx);
385 dot[i].ry = BIG_ENDIAN_SHORT(dot[i].ry);
386
387 if (dot[i].ry == 1023)
388 dot[i].visible = 0;
389 else {
390 dot[i].visible = 1;
391 dot[i].size = 0; /* since we don't know the size, set it as 0 */
392 }
393 }
394 #ifndef GEKKO
395 interpret_ir_data(&wm->ir,&wm->orient,WIIMOTE_IS_SET(wm, WIIMOTE_STATE_ACC));
396 #endif
397 }
398
399 /**
400 * @brief Calculate the data from the IR spots. Extended IR mode.
401 *
402 * @param wm Pointer to a wiimote_t structure.
403 * @param data Data returned by the wiimote for the IR spots.
404 */
calculate_extended_ir(struct wiimote_t * wm,ubyte * data)405 void calculate_extended_ir(struct wiimote_t* wm, ubyte* data) {
406 struct ir_dot_t* dot = wm->ir.dot;
407 int i;
408
409 for (i = 0; i < 4; ++i) {
410 dot[i].rx = 1023 - (data[3*i] | ((data[(3*i)+2] & 0x30) << 4));
411 dot[i].ry = data[(3*i)+1] | ((data[(3*i)+2] & 0xC0) << 2);
412
413 dot[i].size = data[(3*i)+2];
414
415 dot[i].rx = BIG_ENDIAN_SHORT(dot[i].rx);
416 dot[i].ry = BIG_ENDIAN_SHORT(dot[i].ry);
417
418 dot[i].size = dot[i].size&0x0f;
419
420 /* if in range set to visible */
421 if (dot[i].ry == 1023)
422 dot[i].visible = 0;
423 else
424 dot[i].visible = 1;
425 }
426 #ifndef GEKKO
427 interpret_ir_data(&wm->ir,&wm->orient,WIIMOTE_IS_SET(wm, WIIMOTE_STATE_ACC));
428 #endif
429 }
430
431 enum {
432 IR_STATE_DEAD = 0,
433 IR_STATE_GOOD,
434 IR_STATE_SINGLE,
435 IR_STATE_LOST,
436 };
437
438 // half-height of the IR sensor if half-width is 1
439 #define HEIGHT (384.0f / 512.0f)
440 // maximum sensor bar slope (tan(35 degrees))
441 #define MAX_SB_SLOPE 0.7f
442 // minimum sensor bar width in view, relative to half of the IR sensor area
443 #define MIN_SB_WIDTH 0.1f
444 // reject "sensor bars" that happen to have a dot towards the middle
445 #define SB_MIDDOT_REJECT 0.05f
446
447 // physical dimensions
448 // cm center to center of emitters
449 #define SB_WIDTH 19.5f
450 // half-width in cm of emitters
451 #define SB_DOT_WIDTH 2.25f
452 // half-height in cm of emitters (with some tolerance)
453 #define SB_DOT_HEIGHT 1.0f
454
455 #define SB_DOT_WIDTH_RATIO (SB_DOT_WIDTH / SB_WIDTH)
456 #define SB_DOT_HEIGHT_RATIO (SB_DOT_HEIGHT / SB_WIDTH)
457
458 // dots further out than these coords are allowed to not be picked up
459 // otherwise assume something's wrong
460 //#define SB_OFF_SCREEN_X 0.8f
461 //#define SB_OFF_SCREEN_Y (0.8f * HEIGHT)
462
463 // disable, may be doing more harm than good due to sensor pickup glitches
464 #define SB_OFF_SCREEN_X 0.0f
465 #define SB_OFF_SCREEN_Y 0.0f
466
467 // if a point is closer than this to one of the previous SB points
468 // when it reappears, consider it the same instead of trying to guess
469 // which one of the two it is
470 #define SB_SINGLE_NOGUESS_DISTANCE (100.0 * 100.0)
471
472 // width of the sensor bar in pixels at one meter from the Wiimote
473 #define SB_Z_COEFFICIENT 256.0f
474
475 // distance in meters from the center of the FOV to the left or right edge,
476 // when the wiimote is at one meter
477 #define WIIMOTE_FOV_COEFFICIENT 0.39f
478
479 #define SQUARED(x) ((x)*(x))
480 #define WMAX(x,y) ((x>y)?(x):(y))
481 #define WMIN(x,y) ((x<y)?(x):(y))
482
483 /**
484 * @brief Interpret IR data into more user friendly variables.
485 *
486 * @param wm Pointer to a wiimote_t structure.
487 */
find_sensorbar(struct ir_t * ir,struct orient_t * orient)488 void find_sensorbar(struct ir_t* ir, struct orient_t *orient) {
489 struct fdot_t dots[4];
490 struct fdot_t acc_dots[4];
491 struct sb_t cand;
492 struct sb_t candidates[6];
493 struct sb_t sb;
494
495 fdot_t difference;
496
497 int num_candidates = 0;
498
499 int i;
500 int j;
501 int first, second;
502
503 WIIUSE_DEBUG("IR: orient angle: %.02f\n",orient->roll);
504
505 /* count visible dots and populate dots structure */
506 /* dots[] is in -1..1 units for width */
507 ir->num_dots = 0;
508 for (i = 0; i < 4; i++) {
509 if (ir->dot[i].visible) {
510 dots[ir->num_dots].x = (ir->dot[i].rx - 512.0f) / 512.0f;
511 dots[ir->num_dots].y = (ir->dot[i].ry - 384.0f) / 512.0f;
512 WIIUSE_DEBUG("IR: dot %d at (%d,%d) (%.03f,%.03f)\n",ir->num_dots,ir->dot[i].rx,ir->dot[i].ry,dots[ir->num_dots].x,dots[ir->num_dots].y);
513 ir->num_dots++;
514 }
515 }
516
517 WIIUSE_DEBUG("IR: found %d dots\n",ir->num_dots);
518
519 // nothing to track
520 if(ir->num_dots == 0) {
521 if(ir->state != IR_STATE_DEAD)
522 ir->state = IR_STATE_LOST;
523 ir->ax = 0;
524 ir->ay = 0;
525 ir->distance = 0.0f;
526 ir->raw_valid = 0;
527 return;
528 }
529
530 /* ==== Find the Sensor Bar ==== */
531
532 // first rotate according to accelerometer orientation
533 rotate_dots(dots, acc_dots, ir->num_dots, orient->roll);
534 if(ir->num_dots > 1) {
535 WIIUSE_DEBUG("IR: locating sensor bar candidates\n");
536
537 // iterate through all dot pairs
538 for(first=0; first < (ir->num_dots-1); first++) {
539 for(second=(first+1); second < ir->num_dots; second++) {
540 WIIUSE_DEBUG("IR: trying dots %d and %d\n",first,second);
541 // order the dots leftmost first into cand
542 // storing both the raw dots and the accel-rotated dots
543 if(acc_dots[first].x > acc_dots[second].x) {
544 cand.dots[0] = dots[second];
545 cand.dots[1] = dots[first];
546 cand.acc_dots[0] = acc_dots[second];
547 cand.acc_dots[1] = acc_dots[first];
548 } else {
549 cand.dots[0] = dots[first];
550 cand.dots[1] = dots[second];
551 cand.acc_dots[0] = acc_dots[first];
552 cand.acc_dots[1] = acc_dots[second];
553 }
554 difference.x = cand.acc_dots[1].x - cand.acc_dots[0].x;
555 difference.y = cand.acc_dots[1].y - cand.acc_dots[0].y;
556
557 // check angle
558 if(fabsf(difference.y / difference.x) > MAX_SB_SLOPE)
559 continue;
560 WIIUSE_DEBUG("IR: passed angle check\n");
561 // rotate to the true sensor bar angle
562 cand.off_angle = -RAD_TO_DEGREE(atan2(difference.y, difference.x));
563 cand.angle = cand.off_angle + orient->roll;
564 rotate_dots(cand.dots, cand.rot_dots, 2, cand.angle);
565 WIIUSE_DEBUG("IR: off_angle: %.02f, angle: %.02f\n", cand.off_angle, cand.angle);
566 // recalculate x distance - y should be zero now, so ignore it
567 difference.x = cand.rot_dots[1].x - cand.rot_dots[0].x;
568
569 // check distance
570 if(difference.x < MIN_SB_WIDTH)
571 continue;
572 // middle dot check. If there's another source somewhere in the
573 // middle of this candidate, then this can't be a sensor bar
574
575 for(i=0; i<ir->num_dots; i++) {
576 float wadj, hadj;
577 struct fdot_t tdot;
578 if(i==first || i==second) continue;
579 hadj = SB_DOT_HEIGHT_RATIO * difference.x;
580 wadj = SB_DOT_WIDTH_RATIO * difference.x;
581 rotate_dots(&dots[i], &tdot, 1, cand.angle);
582 if( ((cand.rot_dots[0].x + wadj) < tdot.x) &&
583 ((cand.rot_dots[1].x - wadj) > tdot.x) &&
584 ((cand.rot_dots[0].y + hadj) > tdot.y) &&
585 ((cand.rot_dots[0].y - hadj) < tdot.y))
586 break;
587 }
588 // failed middle dot check
589 if(i < ir->num_dots) continue;
590 WIIUSE_DEBUG("IR: passed middle dot check\n");
591
592 cand.score = 1 / (cand.rot_dots[1].x - cand.rot_dots[0].x);
593
594 // we have a candidate, store it
595 WIIUSE_DEBUG("IR: new candidate %d\n",num_candidates);
596 candidates[num_candidates++] = cand;
597 }
598 }
599 }
600
601 if(num_candidates == 0) {
602 int closest = -1;
603 int closest_to = 0;
604 float best = 999.0f;
605 float d;
606 float dx[2];
607 struct sb_t sbx[2];
608 // no sensor bar candidates, try to work with a lone dot
609 WIIUSE_DEBUG("IR: no candidates\n");
610 switch(ir->state) {
611 case IR_STATE_DEAD:
612 WIIUSE_DEBUG("IR: we're dead\n");
613 // we've never seen a sensor bar before, so we're screwed
614 ir->ax = 0.0f;
615 ir->ay = 0.0f;
616 ir->distance = 0.0f;
617 ir->raw_valid = 0;
618 return;
619 case IR_STATE_GOOD:
620 case IR_STATE_SINGLE:
621 case IR_STATE_LOST:
622 WIIUSE_DEBUG("IR: trying to keep track of single dot\n");
623 // try to find the dot closest to the previous sensor bar position
624 for(i=0; i<ir->num_dots; i++) {
625 WIIUSE_DEBUG("IR: checking dot %d (%.02f, %.02f)\n",i, acc_dots[i].x,acc_dots[i].y);
626 for(j=0; j<2; j++) {
627 WIIUSE_DEBUG(" to dot %d (%.02f, %.02f)\n",j, ir->sensorbar.acc_dots[j].x,ir->sensorbar.acc_dots[j].y);
628 d = SQUARED(acc_dots[i].x - ir->sensorbar.acc_dots[j].x);
629 d += SQUARED(acc_dots[i].y - ir->sensorbar.acc_dots[j].y);
630 if(d < best) {
631 best = d;
632 closest_to = j;
633 closest = i;
634 }
635 }
636 }
637 WIIUSE_DEBUG("IR: closest dot is %d to %d\n",closest,closest_to);
638 if(ir->state != IR_STATE_LOST || best < SB_SINGLE_NOGUESS_DISTANCE) {
639 // now work out where the other dot would be, in the acc frame
640 sb.acc_dots[closest_to] = acc_dots[closest];
641 sb.acc_dots[closest_to^1].x = ir->sensorbar.acc_dots[closest_to^1].x - ir->sensorbar.acc_dots[closest_to].x + acc_dots[closest].x;
642 sb.acc_dots[closest_to^1].y = ir->sensorbar.acc_dots[closest_to^1].y - ir->sensorbar.acc_dots[closest_to].y + acc_dots[closest].y;
643 // get the raw frame
644 rotate_dots(sb.acc_dots, sb.dots, 2, -orient->roll);
645 if((fabsf(sb.dots[closest_to^1].x) < SB_OFF_SCREEN_X) && (fabsf(sb.dots[closest_to^1].y) < SB_OFF_SCREEN_Y)) {
646 // this dot should be visible but isn't, since the candidate section failed.
647 // fall through and try to pick out the sensor bar without previous information
648 WIIUSE_DEBUG("IR: dot falls on screen, falling through\n");
649 } else {
650 // calculate the rotated dots frame
651 // angle tends to drift, so recalculate
652 sb.off_angle = -RAD_TO_DEGREE(atan2(sb.acc_dots[1].y - sb.acc_dots[0].y, sb.acc_dots[1].x - sb.acc_dots[0].x));
653 sb.angle = ir->sensorbar.off_angle + orient->roll;
654 rotate_dots(sb.acc_dots, sb.rot_dots, 2, ir->sensorbar.off_angle);
655 WIIUSE_DEBUG("IR: kept track of single dot\n");
656 break;
657 }
658 } else {
659 WIIUSE_DEBUG("IR: lost the dot and new one is too far away\n");
660 }
661 // try to find the dot closest to the sensor edge
662 WIIUSE_DEBUG("IR: trying to find best dot\n");
663 for(i=0; i<ir->num_dots; i++) {
664 d = WMIN(1.0f - fabsf(dots[i].x), HEIGHT - fabsf(dots[i].y));
665 if(d < best) {
666 best = d;
667 closest = i;
668 }
669 }
670 WIIUSE_DEBUG("IR: best dot: %d\n",closest);
671 // now try it as both places in the sensor bar
672 // and pick the one that places the other dot furthest off-screen
673 for(i=0; i<2; i++) {
674 sbx[i].acc_dots[i] = acc_dots[closest];
675 sbx[i].acc_dots[i^1].x = ir->sensorbar.acc_dots[i^1].x - ir->sensorbar.acc_dots[i].x + acc_dots[closest].x;
676 sbx[i].acc_dots[i^1].y = ir->sensorbar.acc_dots[i^1].y - ir->sensorbar.acc_dots[i].y + acc_dots[closest].y;
677 rotate_dots(sbx[i].acc_dots, sbx[i].dots, 2, -orient->roll);
678 dx[i] = WMAX(fabsf(sbx[i].dots[i^1].x),fabsf(sbx[i].dots[i^1].y / HEIGHT));
679 }
680 if(dx[0] > dx[1]) {
681 WIIUSE_DEBUG("IR: dot is LEFT: %.02f > %.02f\n",dx[0],dx[1]);
682 sb = sbx[0];
683 } else {
684 WIIUSE_DEBUG("IR: dot is RIGHT: %.02f < %.02f\n",dx[0],dx[1]);
685 sb = sbx[1];
686 }
687 // angle tends to drift, so recalculate
688 sb.off_angle = -RAD_TO_DEGREE(atan2(sb.acc_dots[1].y - sb.acc_dots[0].y, sb.acc_dots[1].x - sb.acc_dots[0].x));
689 sb.angle = ir->sensorbar.off_angle + orient->roll;
690 rotate_dots(sb.acc_dots, sb.rot_dots, 2, ir->sensorbar.off_angle);
691 WIIUSE_DEBUG("IR: found new dot to track\n");
692 break;
693 }
694 sb.score = 0;
695 ir->state = IR_STATE_SINGLE;
696 } else {
697 int bestidx = 0;
698 float best = 0.0f;
699 WIIUSE_DEBUG("IR: finding best candidate\n");
700 // look for the best candidate
701 // for now, the formula is simple: pick the one with the smallest distance
702 for(i=0; i<num_candidates; i++) {
703 if(candidates[i].score > best) {
704 bestidx = i;
705 best = candidates[i].score;
706 }
707 }
708 WIIUSE_DEBUG("IR: best candidate: %d\n",bestidx);
709 sb = candidates[bestidx];
710 ir->state = IR_STATE_GOOD;
711 }
712
713 ir->raw_valid = 1;
714 ir->ax = ((sb.rot_dots[0].x + sb.rot_dots[1].x) / 2) * 512.0 + 512.0;
715 ir->ay = ((sb.rot_dots[0].y + sb.rot_dots[1].y) / 2) * 512.0 + 384.0;
716 ir->sensorbar = sb;
717 ir->distance = (sb.rot_dots[1].x - sb.rot_dots[0].x) * 512.0;
718
719 }
720
721 #define SMOOTH_IR_RADIUS 8.0f
722 #define SMOOTH_IR_SPEED 0.25f
723 #define SMOOTH_IR_DEADZONE 2.5f
724
725 /**
726 * @brief Smooth the IR pointer position
727 *
728 * @param ir Pointer to an ir_t structure.
729 */
apply_ir_smoothing(struct ir_t * ir)730 void apply_ir_smoothing(struct ir_t *ir) {
731 f32 dx, dy, d, theta;
732
733 WIIUSE_DEBUG("Smooth: OK (%.02f, %.02f) LAST (%.02f, %.02f) ", ir->ax, ir->ay, ir->sx, ir->sy);
734 dx = ir->ax - ir->sx;
735 dy = ir->ay - ir->sy;
736 d = sqrtf(dx*dx + dy*dy);
737 if (d > SMOOTH_IR_DEADZONE) {
738 if (d < SMOOTH_IR_RADIUS) {
739 WIIUSE_DEBUG("INSIDE\n");
740 ir->sx += dx * SMOOTH_IR_SPEED;
741 ir->sy += dy * SMOOTH_IR_SPEED;
742 } else {
743 WIIUSE_DEBUG("OUTSIDE\n");
744 theta = atan2f(dy, dx);
745 ir->sx = ir->ax - cosf(theta) * SMOOTH_IR_RADIUS;
746 ir->sy = ir->ay - sinf(theta) * SMOOTH_IR_RADIUS;
747 }
748 } else {
749 WIIUSE_DEBUG("DEADZONE\n");
750 }
751 }
752
753 // max number of errors before cooked data drops out
754 #define ERROR_MAX_COUNT 8
755 // max number of glitches before cooked data updates
756 #define GLITCH_MAX_COUNT 5
757 // squared delta over which we consider something a glitch
758 #define GLITCH_DIST (150.0f * 150.0f)
759
760 /**
761 * @brief Interpret IR data into more user friendly variables.
762 *
763 * @param ir Pointer to an ir_t structure.
764 * @param orient Pointer to an orient_t structure.
765 */
interpret_ir_data(struct ir_t * ir,struct orient_t * orient)766 void interpret_ir_data(struct ir_t* ir, struct orient_t *orient) {
767
768 float x,y;
769 float d;
770
771 find_sensorbar(ir, orient);
772
773 if(ir->raw_valid) {
774 ir->angle = ir->sensorbar.angle;
775 ir->z = SB_Z_COEFFICIENT / ir->distance;
776 orient->yaw = calc_yaw(ir);
777 if(ir->error_cnt >= ERROR_MAX_COUNT) {
778 ir->sx = ir->ax;
779 ir->sy = ir->ay;
780 ir->glitch_cnt = 0;
781 } else {
782 d = SQUARED(ir->ax - ir->sx) + SQUARED(ir->ay - ir->sy);
783 if(d > GLITCH_DIST) {
784 if(ir->glitch_cnt > GLITCH_MAX_COUNT) {
785 apply_ir_smoothing(ir);
786 ir->glitch_cnt = 0;
787 } else {
788 ir->glitch_cnt++;
789 }
790 } else {
791 ir->glitch_cnt = 0;
792 apply_ir_smoothing(ir);
793 }
794 }
795 ir->smooth_valid = 1;
796 ir->error_cnt = 0;
797 } else {
798 if(ir->error_cnt >= ERROR_MAX_COUNT) {
799 ir->smooth_valid = 0;
800 } else {
801 ir->smooth_valid = 1;
802 ir->error_cnt++;
803 }
804 }
805 if(ir->smooth_valid) {
806 x = ir->sx;
807 y = ir->sy;
808 if (ir_correct_for_bounds(&x, &y, ir->aspect, ir->offset[0], ir->offset[1])) {
809 ir_convert_to_vres(&x, &y, ir->aspect, ir->vres[0], ir->vres[1]);
810 ir->x = x;
811 ir->y = y;
812 ir->valid = 1;
813 } else {
814 ir->valid = 0;
815 }
816 } else {
817 ir->valid = 0;
818 }
819 }
820
821 /**
822 * @brief Calculate yaw given the IR data.
823 *
824 * @param ir IR data structure.
825 */
calc_yaw(struct ir_t * ir)826 float calc_yaw(struct ir_t* ir) {
827 float x;
828
829 x = ir->ax - 512;
830 x *= WIIMOTE_FOV_COEFFICIENT / 512.0;
831
832 return RAD_TO_DEGREE( atanf(x) );
833 }
834