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;
}
}
/*---------------------------------------------------------------------------*/
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();
}
