/*-----------------------------------------------------------------------*/
SWORD read_rev_data(void)
{
u8_t status;
if (MEMS_SUCCESS == GetSatusReg(&status))
{
if (status & 80)
{
GetAngRateRaw(&rev);
rev.x/=1000;
rev.y/=500;
rev.z/=500;
rad.x+=rev.x;
rad.y+=rev.y;
rad.z+=rev.z;
#if 0
LCD_PrintoutInt(32, 0, rev.x);
LCD_PrintoutInt(32, 2, rev.y);
LCD_PrintoutInt(32, 4, rev.z);
#endif
return 0;
}
return 1;
}
return 2;
}
//szenzor kiolvasas
static void prvSensorReadTask (void* pvParameters)
{
portTickType xLastWakeTime;
static uint8_t data[6];
static uint8_t newData;
AccAxesRaw_t AccAxes;
uint8_t status;
int16_t x_acc,y_acc,z_acc;
int16_t x_mag,y_mag,z_mag;
int16_t x_gyr,y_gyr,z_gyr;
xLastWakeTime=xTaskGetTickCount();
while(1)
{
GetSatusReg(&status);
if (status&0b0001000) //new data received
{
newData|=1;
// GetAccAxesRaw(&AccAxes);
readACC(data);
//allithato endiannes miatt olvasható így is
x_acc=((int16_t*)data)[0];
y_acc=((int16_t*)data)[1];
z_acc=((int16_t*)data)[2];
}
ReadStatusM(&status);
if (status&0x01)
{
readMag(data);
x_mag=((int16_t)data[0])<<8;
x_mag|=data[1];
y_mag=((int16_t)data[2])<<8;
y_mag|=data[3];
z_mag=((int16_t)data[4])<<8;
z_mag|=data[5];
}
L3G4200D_GetSatusReg(&status);
if (status&0b00001000)
{
newData|=2;
x_gyr=((int16_t*)data)[0];
y_gyr=((int16_t*)data)[1];
z_gyr=((int16_t*)data)[2];
}
if (newData==3)
{
newData=0;
}
vTaskDelayUntil(&xLastWakeTime,100);
}
}
