void L3GD20H::Read( Vector3f* v, bool raw )
{
short sgyro[3] = { 0 };
mI2C->Read( L3GD20_OUT_X_L | 0x80, sgyro, sizeof(sgyro) );
v->x = 0.0703125f * (float)sgyro[0];
v->y = 0.0703125f * (float)sgyro[1];
v->z = 0.0703125f * (float)sgyro[2];
v->operator-=( mOffset );
ApplySwap( *v );
mLastValues = *v;
}
void MPU9150Gyro::Read( Vector3f* v, bool raw )
{
// short sgyro[3] = { 0 };
uint8_t sgyro[6] = { 0 };
mI2C->Read( MPU_9150_GYRO_XOUT_H | 0x80, sgyro, sizeof(sgyro) );
v->x = (float)( (int16_t)( sgyro[0] << 8 | sgyro[1] ) ) * 0.061037018952f;
v->y = (float)( (int16_t)( sgyro[2] << 8 | sgyro[3] ) ) * 0.061037018952f;
v->z = (float)( (int16_t)( sgyro[4] << 8 | sgyro[5] ) ) * 0.061037018952f;
ApplySwap( *v );
if ( not raw ) {
v->x -= mOffset.x;
v->y -= mOffset.y;
v->z -= mOffset.z;
}
mLastValues = *v;
}
void MPU9150Accel::Read( Vector3f* v, bool raw )
{
// short saccel[3] = { 0 };
uint8_t saccel[6] = { 0 };
mI2C->Read( MPU_9150_ACCEL_XOUT_H | 0x80, saccel, sizeof(saccel) );
v->x = (float)( (int16_t)( saccel[0] << 8 | saccel[1] ) ) * 8.0f * 6.103515625e-04f;
v->y = (float)( (int16_t)( saccel[2] << 8 | saccel[3] ) ) * 8.0f * 6.103515625e-04f;
v->z = (float)( (int16_t)( saccel[4] << 8 | saccel[5] ) ) * 8.0f * 6.103515625e-04f;
ApplySwap( *v );
if ( not raw ) {
v->x -= mOffset.x;
v->y -= mOffset.y;
}
mLastValues = *v;
}
