struct wmplugin_data *wmplugin_exec(int mesg_count, union cwiid_mesg mesg[], struct timespec *timestamp)
{
int i;
enum cwiid_ext_type ext_type = CWIID_EXT_NONE;
struct wmplugin_data *ret = NULL;
for (i=0; i < mesg_count; i++) {
switch (mesg[i].type) {
case CWIID_MESG_STATUS:
if ((mesg[i].status_mesg.ext_type == CWIID_EXT_NUNCHUK) &&
(ext_type != CWIID_EXT_NUNCHUK)) {
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NUNCHUK, &acc_cal)) {
wmplugin_err(plugin_id, "calibration error");
}
}
ext_type = mesg[i].status_mesg.ext_type;
break;
case CWIID_MESG_NUNCHUK:
process_nunchuk(&mesg[i].nunchuk_mesg);
ret = &data;
break;
default:
break;
}
}
return ret;
}
static void process_motionplus(struct cwiid_motionplus_mesg *mesg)
{
rollVel = (- mesg->angle_rate[1] + motionplus_cal.zero[1])/20;
// Todo not dummy calibration
yawVel = (-mesg->angle_rate[2] + motionplus_cal.zero[2])/20; // in deg/s
pitchVel = (mesg->angle_rate[0] - motionplus_cal.zero[0])/20;
if(!mesg->low_speed[2])
yawVel *= 5;
if(!mesg->low_speed[0])
pitchVel *=5;
if(!mesg->low_speed[1])
rollVel *=5;
rollVel = (fabs(rollVel)>0.5)?rollVel:0;
yawVel = (fabs(yawVel)>0.5)?yawVel:0;
pitchVel = (fabs(pitchVel)>0.5)?pitchVel:0;
// if( fabs(rollVel-old_rollVel)<20 && fabs(yawVel-old_yawVel)<20 && fabs(pitchVel-old_pitchVel)<20 ){
//
// motionplus_cal.zero[0] = mesg->angle_rate[0]*NEW_AMOUNT +motionplus_cal.zero[0]*OLD_AMOUNT;
// motionplus_cal.zero[1] = mesg->angle_rate[1]*NEW_AMOUNT +motionplus_cal.zero[1]*OLD_AMOUNT;
// motionplus_cal.zero[2] = mesg->angle_rate[2]*NEW_AMOUNT +motionplus_cal.zero[2]*OLD_AMOUNT;
// wmplugin_err(plugin_id," Changed cal (%i,%i,%i)", motionplus_cal.zero[0], motionplus_cal.zero[1], motionplus_cal.zero[2]);
// }
// SAMPLE RATE NEEDS TO BE STABLE.. we chose 10ms
wmplugin_err(plugin_id, " UPDATED PITCH %g", sigmoid(pitchVel - PITCH_STARTGYRO)*.01*PI/180);
roll += sigmoid(rollVel - ROLL_STARTGYRO)*.01*PI/180;
yaw += yawVel*.01*PI/180;
pitch += sigmoid(pitchVel - PITCH_STARTGYRO)*.01*PI/180;
}
int wmplugin_init(int id, cwiid_wiimote_t *wiimote)
{
plugin_id = id;
ccd_center.valid=1;
ccd_center.size=1;
ccd_center.pos[0]=512;
ccd_center.pos[1]=384;
motionplus_cal.zero[0] = 8050;
motionplus_cal.zero[1] = 8000;
motionplus_cal.zero[2] = 8580;
data.buttons = 0;
data.axes[0].valid = 1;
data.axes[1].valid = 1;
data.axes[2].valid = 1;
data.axes[3].valid = 1;
data.axes[4].valid = 1;
data.axes[4].value = 0;
data.axes[5].valid = 1;
if (wmplugin_set_rpt_mode(id, CWIID_RPT_ACC | CWIID_RPT_MOTIONPLUS | CWIID_RPT_IR | CWIID_RPT_BTN)) {
return -1;
}
if(cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS)) {
wmplugin_err(id, "You need wiimotion plus to run this plugin");
return -1;
}
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &acc_cal)) {
wmplugin_err(id, "accelerometers calibration error");
return -1;
}
return 0;
}
int wmplugin_init(int id, cwiid_wiimote_t *wiimote)
{
plugin_id = id;
data.buttons = 0;
data.axes[0].valid = 1;
data.axes[1].valid = 1;
if (wmplugin_set_rpt_mode(id, CWIID_RPT_ACC)) {
return -1;
}
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &acc_cal)) {
wmplugin_err(id, "calibration error");
return -1;
}
return 0;
}
static void process_ir(struct cwiid_ir_mesg *mesg)
{
int src_index1 = -1;
int src_index2 = -1;
int i = 0;
double distance,ir_size,new_ir_size;
//detect 2 brightest points (LED bar)
for (i=0; i < CWIID_IR_SRC_COUNT; i++) {
if (mesg->src[i].valid) {
if ((src_index1 == -1) ||
(mesg->src[i].size > mesg->src[src_index1].size)) {
src_index1 = i;
}else if ((src_index2 == -1) ||
(mesg->src[i].size > mesg->src[src_index2].size)) {
src_index2 = i;
}
}
}
if ( src_index1 == -1 || src_index2 == -1 || !data.axes[4].valid){
wmplugin_err(plugin_id,"Not enough LEDs");
return;
}
//computing distance of sources on virtual screen
ir_size = sqrt(pow(mesg->src[src_index1].pos[0] - mesg->src[src_index2].pos[0],2)
+ pow(mesg->src[src_index1].pos[1] - mesg->src[src_index2].pos[1],2));
if (ir_size==0)
return;
// D = f (L' / l + 1)
distance = CWIID_FOCAL_LENGTH * CWIID_LEDBAR_SIZE / ir_size;
//TODO Transform Infrared dots on {real screen} to {virtual
// roll=0, yaw=0, pitch=0 screen}.
static struct cwiid_ir_src virtual_led1;
static struct cwiid_ir_src virtual_led2;
//Remove Roll first
virtual_led1.size=mesg->src[src_index1].size;
virtual_led1.pos[0] = mesg->src[src_index1].pos[0]*cos(-roll) + mesg->src[src_index1].pos[1]*sin(roll);
virtual_led1.pos[1] = mesg->src[src_index1].pos[0]*sin(-roll) + mesg->src[src_index1].pos[1]*cos(-roll);
virtual_led2.size=mesg->src[src_index2].size;
virtual_led2.pos[0] = mesg->src[src_index2].pos[0]*cos(-roll) + mesg->src[src_index2].pos[1]*sin(roll);
virtual_led2.pos[1] = mesg->src[src_index2].pos[0]*sin(-roll) + mesg->src[src_index2].pos[1]*cos(-roll);
//Check led image is flat (not y component left), if not correct error in roll
if (virtual_led1.pos[0]-virtual_led2.pos[0] == 0)
wmplugin_err(plugin_id," Warning invalid roll correction (have 90 deg, roll %g)",
roll*180/PI);
else if(fabs(atan((virtual_led1.pos[1]-virtual_led2.pos[1])/(virtual_led1.pos[0]-virtual_led2.pos[0]))*180/PI) > 5)
//TODO correction if false
wmplugin_err(plugin_id," Warning invalid roll correction (have %g deg, roll %g) ",
fabs(atan((virtual_led1.pos[1]-virtual_led2.pos[1])/(virtual_led1.pos[0]-virtual_led2.pos[0]))*180/PI),roll*180/PI,
virtual_led1.pos[0],virtual_led1.pos[1],virtual_led2.pos[0],virtual_led2.pos[1]);
//Now remove yaw and pitch at the same time, it's easy they are independant now.
virtual_led1.pos[0] = CWIID_FOCAL_LENGTH*tan(atan(virtual_led1.pos[0]/CWIID_FOCAL_LENGTH)-yaw);
virtual_led1.pos[1] = CWIID_FOCAL_LENGTH*tan(atan(virtual_led1.pos[1]/CWIID_FOCAL_LENGTH)-pitch);
virtual_led2.pos[0] = CWIID_FOCAL_LENGTH*tan(atan(virtual_led2.pos[0]/CWIID_FOCAL_LENGTH)-yaw);
virtual_led2.pos[1] = CWIID_FOCAL_LENGTH*tan(atan(virtual_led2.pos[1]/CWIID_FOCAL_LENGTH)-pitch);
new_ir_size = sqrt(pow(virtual_led1.pos[0] - virtual_led2.pos[0],2)
+ pow(virtual_led1.pos[1] - virtual_led2.pos[1],2));
if(fabs(new_ir_size-ir_size)>50){ //check we didnt' screw anything with our corrections
wmplugin_err(plugin_id," Warning invalid correction (difference in size %g) ir_size:%g , new:%g \n old:(%i,%i) (%i,%i) \nnew:(%i,%i) (%i,%i)"
,fabs(new_ir_size-ir_size),ir_size,new_ir_size,
led1.pos[0],led1.pos[1],led2.pos[0],led2.pos[1],
virtual_led1.pos[0],virtual_led1.pos[1],virtual_led2.pos[0],virtual_led2.pos[1]);
return;
}
// Now we have corrected orientation, let's compute our position relative to led1
x = CWIID_LEDBAR_SIZE/ir_size*(virtual_led1.pos[0]-ccd_center.pos[0]);
y = CWIID_LEDBAR_SIZE/ir_size*(virtual_led1.pos[1]-ccd_center.pos[1]);
z = sqrt( pow(distance,2) - pow(x,2) - pow(y,2));
}
struct wmplugin_data *wmplugin_exec(int mesg_count, union cwiid_mesg mesg[], struct timespec* timestamp)
{
double old_x=x;
double old_y=y;
double old_z=z;
double old_roll = roll;
double old_yaw = yaw;
double old_pitch = pitch;
int i;
struct wmplugin_data *ret = NULL;
for (i=0; i < mesg_count; i++) {
switch (mesg[i].type) {
case CWIID_MESG_ACC:
process_acc(&mesg[i].acc_mesg);
ret = &data;
break;
case CWIID_MESG_MOTIONPLUS:
process_motionplus(&mesg[i].motionplus_mesg);
ret = &data;
break;
case CWIID_MESG_IR:
process_ir(&mesg[i].ir_mesg);
ret = &data;
break;
case CWIID_MESG_BTN:
if( do_process && mesg[i].btn_mesg.buttons == CWIID_BTN_A)
do_process=0;
else
do_process=1;
default:
break;
}
}
if(roll>PI)
roll = PI;
else if(roll<-PI)
roll = -PI;
if(yaw>PI)
yaw = PI;
else if(yaw<-PI)
yaw = -PI;
if(pitch>PI/2)
pitch = PI/2;
else if(pitch<-PI/2)
pitch = -PI/2;
wmplugin_err(plugin_id,"XYZ(%g,%g,%g) roll, yaw, pitch(%g,%g,%g) \n", x, y, z, roll*180/PI, yaw*180/PI, pitch*180/PI);
wmplugin_err(plugin_id,"XYZ(%g,%g,%g) roll, yaw, pitch(%g,%g,%g) \n", old_x, old_y, old_z, old_roll*180/PI, old_yaw*180/PI, old_pitch*180/PI);
if (do_process){
for (i=0 ;i<6;i++)
data.axes[i].valid=1;
data.axes[0].value = (old_x-x) /10/ X_Scale;
data.axes[1].value = (y-old_y) /10/ Y_Scale;
data.axes[2].value = (old_z-z) /10/ Z_Scale;
// data.axes[3].value = (roll- old_roll) *180/PI /Roll_Scale;
data.axes[3].value = rollVel /10 /Roll_Scale;
data.axes[4].value = yawVel /10 /Yaw_Scale;
data.axes[5].value = pitchVel /10 /Pitch_Scale;
// data.axes[5].value = (pitch- old_pitch) *180/PI /Pitch_Scale;
wmplugin_err(plugin_id,"COMMANDS XYZ(%d,%d,%d) roll, yaw, pitch(%d,%d,%d) \n", data.axes[0].value, data.axes[1].value, data.axes[2].value, data.axes[3].value, data.axes[4].value, data.axes[5].value);
}else
for (i=0 ;i<6;i++)
data.axes[i].valid=0;
return ret;
}
