/**
* @brief Interpret IR data into more user friendly variables.
*
* @param wm Pointer to a wiimote_t structure.
*/
static void interpret_ir_data(struct wiimote_t* wm) {
struct ir_dot_t* dot = wm->ir.dot;
int i;
float roll = 0.0f;
int last_num_dots = wm->ir.num_dots;
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_ACC))
roll = wm->orient.roll;
/* count visible dots */
wm->ir.num_dots = 0;
for (i = 0; i < 4; ++i) {
if (dot[i].visible)
wm->ir.num_dots++;
}
switch (wm->ir.num_dots) {
case 0:
{
wm->ir.state = 0;
/* reset the dot ordering */
for (i = 0; i < 4; ++i)
dot[i].order = 0;
wm->ir.x = 0;
wm->ir.y = 0;
wm->ir.z = 0.0f;
return;
}
case 1:
{
fix_rotated_ir_dots(wm->ir.dot, roll);
if (wm->ir.state < 2) {
/*
* Only 1 known dot, so use just that.
*/
for (i = 0; i < 4; ++i) {
if (dot[i].visible) {
wm->ir.x = dot[i].x;
wm->ir.y = dot[i].y;
wm->ir.ax = wm->ir.x;
wm->ir.ay = wm->ir.y;
/* can't calculate yaw because we don't have the distance */
//wm->orient.yaw = calc_yaw(&wm->ir);
ir_convert_to_vres(&wm->ir.x, &wm->ir.y, wm->ir.aspect, wm->ir.vres[0], wm->ir.vres[1]);
break;
}
}
} else {
/*
* Only see 1 dot but know theres 2.
* Try to estimate where the other one
* should be and use that.
*/
for (i = 0; i < 4; ++i) {
if (dot[i].visible) {
int ox = 0;
int x, y;
if (dot[i].order == 1)
/* visible is the left dot - estimate where the right is */
ox = dot[i].x + wm->ir.distance;
else if (dot[i].order == 2)
/* visible is the right dot - estimate where the left is */
ox = dot[i].x - wm->ir.distance;
x = ((signed int)dot[i].x + ox) / 2;
y = dot[i].y;
wm->ir.ax = x;
wm->ir.ay = y;
wm->orient.yaw = calc_yaw(&wm->ir);
if (ir_correct_for_bounds(&x, &y, wm->ir.aspect, wm->ir.offset[0], wm->ir.offset[1])) {
ir_convert_to_vres(&x, &y, wm->ir.aspect, wm->ir.vres[0], wm->ir.vres[1]);
wm->ir.x = x;
wm->ir.y = y;
}
break;
}
}
}
break;
}
case 2:
case 3:
case 4:
{
/*
* Two (or more) dots known and seen.
* Average them together to estimate the true location.
*/
int x, y;
wm->ir.state = 2;
fix_rotated_ir_dots(wm->ir.dot, roll);
/* if there is at least 1 new dot, reorder them all */
if (wm->ir.num_dots > last_num_dots) {
reorder_ir_dots(dot);
wm->ir.x = 0;
wm->ir.y = 0;
}
wm->ir.distance = ir_distance(dot);
wm->ir.z = 1023 - wm->ir.distance;
get_ir_dot_avg(wm->ir.dot, &x, &y);
wm->ir.ax = x;
wm->ir.ay = y;
wm->orient.yaw = calc_yaw(&wm->ir);
if (ir_correct_for_bounds(&x, &y, wm->ir.aspect, wm->ir.offset[0], wm->ir.offset[1])) {
ir_convert_to_vres(&x, &y, wm->ir.aspect, wm->ir.vres[0], wm->ir.vres[1]);
wm->ir.x = x;
wm->ir.y = y;
}
break;
}
default:
{
break;
}
}
#ifdef WITH_WIIUSE_DEBUG
{
int ir_level;
WIIUSE_GET_IR_SENSITIVITY(wm, &ir_level);
WIIUSE_DEBUG("IR sensitivity: %i", ir_level);
WIIUSE_DEBUG("IR visible dots: %i", wm->ir.num_dots);
for (i = 0; i < 4; ++i)
if (dot[i].visible)
WIIUSE_DEBUG("IR[%i][order %i] (%.3i, %.3i) -> (%.3i, %.3i)", i, dot[i].order, dot[i].rx, dot[i].ry, dot[i].x, dot[i].y);
WIIUSE_DEBUG("IR[absolute]: (%i, %i)", wm->ir.x, wm->ir.y);
}
#endif
}
/**
* @brief Interpret IR data into more user friendly variables.
*
* @param wm Pointer to a wiimote_t structure.
*/
bool interpret_ir_data(wiimote& wm, float& out_x, float& out_y, float& out_z)
{
wiimote::ir::dot* dot = wm.IR.Dot;
int i;
float roll = 0.0f;
int last_num_dots = ir_num_dots;
roll = wm.Acceleration.Orientation.Roll;
/* count visible dots */
ir_num_dots = 0;
for (i = 0; i < 4; ++i) {
if (dot[i].bFound)
ir_num_dots++;
}
switch (ir_num_dots) {
case 0:
{
ir_state = 0;
/* reset the dot ordering */
for (i = 0; i < 4; ++i)
dots_order[i] = 0;
out_x = 0.0f;
out_y = 0.0f;
out_z = 0.0f;
return 0;
}
case 1:
{
fix_rotated_ir_dots(wm.IR.Dot, roll);
if (ir_state < 2) {
/*
* Only 1 known dot, so use just that.
*/
for (i = 0; i < 4; ++i) {
if (dot[i].bFound) {
out_x = dot[i].X;
out_y = dot[i].Y;
break;
}
}
} else {
/*
* Only see 1 dot but know theres 2.
* Try to estimate where the other one
* should be and use that.
*/
for (i = 0; i < 4; ++i) {
if (dot[i].bFound) {
float ox, x, y;
if (dots_order[i] == 1)
/* visible is the left dot - estimate where the right is */
ox = dot[i].X + ir_distance;
else if (dots_order[i] == 2)
/* visible is the right dot - estimate where the left is */
ox = dot[i].X - ir_distance;
x = (dot[i].X + ox) / 2;
y = dot[i].Y;
out_x = x;
out_y = y;
break;
}
}
}
return 1;
}
case 2:
case 3:
case 4:
{
/*
* Two (or more) dots known and seen.
* Average them together to estimate the true location.
*/
float x, y;
ir_state = 2;
fix_rotated_ir_dots(wm.IR.Dot, roll);
/* if there is at least 1 new dot, reorder them all */
if (ir_num_dots > last_num_dots) {
reorder_ir_dots(dot);
out_x = 0.0f;
out_y = 0.0f;
}
ir_distance = cal_ir_distance(dot);
out_z = 1.0f - ir_distance;
get_ir_dot_avg(wm.IR.Dot, x, y);
out_x = x;
out_y = y;
return 1;
}
default:
{
return 0;
}
}
return 1;
}
