void readITG3200(int i2c)
{
char buf[8];
static int reported = 0;
selectDevice(i2c, ITG3200_I2C_ADDR, "ITG3200");
writeToDevice(i2c, "\x1D", 1);
if (read(i2c, buf, 6) != 6)
{
if (!reported)
{
fprintf(stderr, "Unable to read from ITG3200\n");
reported = 1;
}
}
else
{
reported = 0;
GYRO_ORIENTATION (
((buf[0]<<8) | buf[1]),
((buf[2]<<8) | buf[3]),
((buf[4]<<8) | buf[5]) );
}
}
void SensorReadGYRO()
{
#if STACK_GYRO
int16_t rawGYRO[3];
#ifdef MPU6050
MPU6050_getRotation(&rawGYRO[0],&rawGYRO[1], &rawGYRO[2]);
#endif
GYRO_ORIENTATION(rawGYRO[0],rawGYRO[1],rawGYRO[2]);
//printf("Raw GYRO:%d %d %d\n",Sensor.rawGYRO[0], Sensor.rawGYRO[1], Sensor.rawGYRO[2]);
#endif
}
void read_gyro() {
unsigned char data[6];
lib_i2c_readdata(MPU6050_ADDRESS,0x43,(unsigned char *)&data,6);
// convert to fixedpointnum, in degrees per second
// the gyro puts out an int where each count equals 0.0609756097561 degrees/second
// we want fixedpointnums, so we multiply by 3996 (0.0609756097561 * (1<<FIXEDPOINTSHIFT))
GYRO_ORIENTATION(global.gyrorate,((data[0]<<8) | data[1])*3996L , // range: +/- 8192; +/- 2000 deg/sec
((data[2]<<8) | data[3])*3996L ,
((data[4]<<8) | data[5])*3996L );
}
void read_gyro() {
unsigned char data[6];
lib_i2c_readdata(ITG3200_ADDRESS,0X1D,(unsigned char *)&data,6);
// convert to fixedpointnum, in degrees per second
// the gyro puts out an int where each count equals 0.0695652173913 degrees/second
// we want fixedpointnums, so we multiply by 4559 (0.0695652173913 * (1<<FIXEDPOINTSHIFT))
GYRO_ORIENTATION(global.gyrorate,((data[0]<<8) | data[1])*4559L , // range: +/- 8192; +/- 2000 deg/sec
((data[2]<<8) | data[3])*4559L ,
((data[4]<<8) | data[5])*4559L );
}
void readgyro(void)
{
unsigned char data[6];
lib_i2c_readdata(MPU3050_ADDRESS, 0x1D, (unsigned char *) &data, 6);
// convert to fixedpointnum, in degrees per second
// the gyro puts out a 16 bit signed int where each count equals 0.0609756097561 degrees/second
// So we have 15 bit fractional part, need to shift that to FIXEDPOINTSHIFT and
// take 2000deg/s into account.
// This only works if FIXEDPOINTSHIFT >= 15
GYRO_ORIENTATION(global.gyrorate,
((int16_t) ((data[0] << 8) | data[1])) * (2000L << (FIXEDPOINTSHIFT - 15)),
((int16_t) ((data[2] << 8) | data[3])) * (2000L << (FIXEDPOINTSHIFT - 15)),
((int16_t) ((data[4] << 8) | data[5])) * (2000L << (FIXEDPOINTSHIFT - 15)));
}
void SensorReadGYRO()
{
#if STACK_GYRO
int16_t rawGYRO[3];
#if defined(MPU6050) || defined(MPU6500)
MPU6050_getRotation(&rawGYRO[0],&rawGYRO[1], &rawGYRO[2]);
#else
rawGYRO[0] = readRawGyroX();
rawGYRO[1] = readRawGyroY();
rawGYRO[2] = readRawGyroZ();
#endif
GYRO_ORIENTATION(rawGYRO[0],rawGYRO[1],rawGYRO[2]);
//printf("Raw GYRO:%d %d %d\n",Sensor.rawGYRO[0], Sensor.rawGYRO[1], Sensor.rawGYRO[2]);
#endif
}
