int Sensors_Process(uint8_t SensorType, float dt)
{
switch (SensorType)
{
case GYRO:
if (MARG_SENSORS[GYRO]->SensorValueUpdated)
{
/*ITG 3200*/
float x = (float) ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[1]) | (MARG_SENSORS[GYRO]->SensorRawValue[0] << 8))));
float y = (float) ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[3]) | (MARG_SENSORS[GYRO]->SensorRawValue[2] << 8))));
float z = (float) ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[5]) | (MARG_SENSORS[GYRO]->SensorRawValue[4] << 8))));
//float x = (float)((int)((int16_t)((MARG_SENSORS[GYRO]->SensorRawValue[0]) | (MARG_SENSORS[GYRO]->SensorRawValue[1] <<8))));
//float y = (float)((int)((int16_t)((MARG_SENSORS[GYRO]->SensorRawValue[2]) | (MARG_SENSORS[GYRO]->SensorRawValue[3] <<8))));
//float z = (float)((int)((int16_t)((MARG_SENSORS[GYRO]->SensorRawValue[4]) | (MARG_SENSORS[GYRO]->SensorRawValue[5] <<8))));
MARG_SENSORS[GYRO]->SensorValueUpdated = -1;
//async_printf("GYRO: %f %f %f\r\n",x, y, z);
process_gyro(x, y, z, dt);
GPIOToggle(LED6);
return 1;
}
break;
case ACC:
if (MARG_SENSORS[ACC]->SensorValueUpdated)
{
float x = (float) ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[0]) | (MARG_SENSORS[ACC]->SensorRawValue[1] << 8))));
float y = (float) ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[2]) | (MARG_SENSORS[ACC]->SensorRawValue[3] << 8))));
float z = (float) ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[4]) | (MARG_SENSORS[ACC]->SensorRawValue[5] << 8))));
MARG_SENSORS[ACC]->SensorValueUpdated = -1;
//async_printf("ACC: %f %f %f\r\n", x, y, z);
process_acc(x, y, z);
GPIOToggle(LED6);
return 1;
}
break;
case MAG:
if (MARG_SENSORS[MAG]->SensorValueUpdated)
{
float x = (float) ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[1]) | (MARG_SENSORS[MAG]->SensorRawValue[0] << 8))));
float y = (float) ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[3]) | (MARG_SENSORS[MAG]->SensorRawValue[2] << 8))));
float z = (float) ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[5]) | (MARG_SENSORS[MAG]->SensorRawValue[4] << 8))));
MARG_SENSORS[MAG]->SensorValueUpdated = -1;
//async_printf("MAG: %f %f %f\r\n",x, y, z);
process_mag(x, y, z);
GPIOToggle(LED6);
return 1;
}
break;
default:
return -1;
}
}
struct wmplugin_data *wmplugin_exec(int mesg_count, union cwiid_mesg mesg[])
{
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;
default:
break;
}
}
return ret;
}
/*---------------------------------------------------------------------------*/
PROCESS_THREAD(sensor_collection, ev, data)
{
PROCESS_BEGIN();
static struct etimer timer;
#if IOTLAB_M3
config_light();
config_pressure();
#endif
config_acc();
config_mag();
config_gyr();
etimer_set(&timer, CLOCK_SECOND);
while(1) {
PROCESS_WAIT_EVENT();
if (ev == PROCESS_EVENT_TIMER) {
#if IOTLAB_M3
process_light();
process_pressure();
#endif
etimer_restart(&timer);
} else if (ev == sensors_event && data == &acc_sensor) {
process_acc();
} else if (ev == sensors_event && data == &mag_sensor) {
process_mag();
} else if (ev == sensors_event && data == &gyr_sensor) {
process_gyr();
}
}
PROCESS_END();
}
void *router_thread(struct wiimote *wiimote)
{
unsigned char buf[READ_BUF_LEN];
ssize_t len;
struct mesg_array ma;
char err, print_clock_err = 1;
while (1) {
/* Read packet */
len = read(wiimote->int_socket, buf, READ_BUF_LEN);
ma.count = 0;
if (clock_gettime(CLOCK_REALTIME, &ma.timestamp)) {
if (print_clock_err) {
cwiid_err(wiimote, "clock_gettime error");
print_clock_err = 0;
}
}
err = 0;
if ((len == -1) || (len == 0)) {
process_error(wiimote, len, &ma);
write_mesg_array(wiimote, &ma);
/* Quit! */
break;
}
else {
/* Verify first byte (DATA/INPUT) */
if (buf[0] != (BT_TRANS_DATA | BT_PARAM_INPUT)) {
cwiid_err(wiimote, "Invalid packet type");
}
/* Main switch */
/* printf("%.2X %.2X %.2X %.2X %.2X %.2X %.2X %.2X\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
printf("%.2X %.2X %.2X %.2X %.2X %.2X %.2X %.2X\n", buf[8], buf[9], buf[10], buf[11], buf[12], buf[13], buf[14], buf[15]);
printf("%.2X %.2X %.2X %.2X %.2X %.2X %.2X %.2X\n", buf[16], buf[17], buf[18], buf[19], buf[20], buf[21], buf[22], buf[23]);
printf("\n"); */
switch (buf[1]) {
case RPT_STATUS:
err = process_status(wiimote, &buf[2], &ma);
break;
case RPT_BTN:
err = process_btn(wiimote, &buf[2], &ma);
break;
case RPT_BTN_ACC:
err = process_btn(wiimote, &buf[2], &ma) ||
process_acc(wiimote, &buf[4], &ma);
break;
case RPT_BTN_EXT8:
err = process_btn(wiimote, &buf[2], &ma) ||
process_ext(wiimote, &buf[4], 8, &ma);
break;
case RPT_BTN_ACC_IR12:
err = process_btn(wiimote, &buf[2], &ma) ||
process_acc(wiimote, &buf[4], &ma) ||
process_ir12(wiimote, &buf[7], &ma);
break;
case RPT_BTN_EXT19:
err = process_btn(wiimote, &buf[2], &ma) ||
process_ext(wiimote, &buf[4], 19, &ma);
break;
case RPT_BTN_ACC_EXT16:
err = process_btn(wiimote, &buf[2], &ma) ||
process_acc(wiimote, &buf[4], &ma) ||
process_ext(wiimote, &buf[7], 16, &ma);
break;
case RPT_BTN_IR10_EXT9:
err = process_btn(wiimote, &buf[2], &ma) ||
process_ir10(wiimote, &buf[4], &ma) ||
process_ext(wiimote, &buf[14], 9, &ma);
break;
case RPT_BTN_ACC_IR10_EXT6:
err = process_btn(wiimote, &buf[2], &ma) ||
process_acc(wiimote, &buf[4], &ma) ||
process_ir10(wiimote, &buf[7], &ma) ||
process_ext(wiimote, &buf[17], 6, &ma);
break;
case RPT_EXT21:
err = process_ext(wiimote, &buf[2], 21, &ma);
break;
case RPT_BTN_ACC_IR36_1:
case RPT_BTN_ACC_IR36_2:
cwiid_err(wiimote, "Unsupported report type received "
"(interleaved data)");
err = 1;
break;
case RPT_READ_DATA:
err = process_read(wiimote, &buf[4]) ||
process_btn(wiimote, &buf[2], &ma);
break;
case RPT_WRITE_ACK:
err = process_write(wiimote, &buf[2]);
break;
default:
cwiid_err(wiimote, "Unknown message type");
err = 1;
break;
}
if (!err && (ma.count > 0)) {
if (update_state(wiimote, &ma)) {
cwiid_err(wiimote, "State update error");
}
if (wiimote->flags & CWIID_FLAG_MESG_IFC) {
/* prints its own errors */
write_mesg_array(wiimote, &ma);
}
}
}
}
return NULL;
}
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;
}
