void enableIMU()
{
__u16 block[I2C_SMBUS_BLOCK_MAX];
int res, bus, size;
char filename[20];
sprintf(filename, "/dev/i2c-%d", 1);
file = open(filename, O_RDWR);
if (file<0) {
printf("Unable to open I2C bus!");
exit(1);
}
// Enable accelerometer.
writeAccReg(CTRL_REG1_XM, 0b01100111); // z,y,x axis enabled, continuos update, 100Hz data rate
writeAccReg(CTRL_REG2_XM, 0b00100000); // +/- 16G full scale
//Enable the magnetometer
writeMagReg( CTRL_REG5_XM, 0b11110000); // Temp enable, M data rate = 50Hz
writeMagReg( CTRL_REG6_XM, 0b01100000); // +/-12gauss
writeMagReg( CTRL_REG7_XM, 0b00000000); // Continuous-conversion mode
// Enable Gyro
writeGyrReg(CTRL_REG1_G, 0b00001111); // Normal power mode, all axes enabled
writeGyrReg(CTRL_REG4_G, 0b00110000); // Continuos update, 2000 dps full scale
}
void enableSensors()
{
__u16 block[I2C_SMBUS_BLOCK_MAX];
int res, bus, size;
char filename[20];
sprintf( filename, "/dev/i2c-%d", 1 );
file = open( filename, O_RDWR );
if( file < 0 )
{
printf( "Unable to open I2C bus!" );
exit( 1 );
}
// Accelerometer
writeAccReg( CTRL_REG1_XM, 0b01100111 ); // Enable all axes at 100Hz
writeAccReg( CTRL_REG2_XM, 0b00100000 ); // +/- 16G Accel
// Gyroscope
writeGyrReg( CTRL_REG1_G, 0b00001111 ); // Enable all axes at 100Hz
writeGyrReg( CTRL_REG4_G, 0b00110000 ); // +/- 2000DPS
// Magnetometer
writeMagReg( CTRL_REG5_XM, 0b11110000 ); // Thermometer enable at 50Hz
writeMagReg( CTRL_REG6_XM, 0b01100000 ); // +/- 12 Gauss
writeMagReg( CTRL_REG7_XM, 0b00000000 ); // Auto update
}
void enableIMU()
{
if (LSM9DS0){//For BerryIMUv1
// Enable accelerometer.
writeAccReg(LSM9DS0_CTRL_REG1_XM, 0b01100111); // z,y,x axis enabled, continuous update, 100Hz data rate
writeAccReg(LSM9DS0_CTRL_REG2_XM, 0b00100000); // +/- 16G full scale
//Enable the magnetometer
writeMagReg(LSM9DS0_CTRL_REG5_XM, 0b11110000); // Temp enable, M data rate = 50Hz
writeMagReg(LSM9DS0_CTRL_REG6_XM, 0b01100000); // +/-12gauss
writeMagReg(LSM9DS0_CTRL_REG7_XM, 0b00000000); // Continuous-conversion mode
// Enable Gyro
writeGyrReg(LSM9DS0_CTRL_REG1_G, 0b00001111); // Normal power mode, all axes enabled
writeGyrReg(LSM9DS0_CTRL_REG4_G, 0b00110000); // Continuos update, 2000 dps full scale
}
if (LSM9DS1){//For BerryIMUv2
// Enable the gyroscope
writeGyrReg(LSM9DS1_CTRL_REG4,0b00111000); // z, y, x axis enabled for gyro
writeGyrReg(LSM9DS1_CTRL_REG1_G,0b10111000); // Gyro ODR = 476Hz, 2000 dps
writeGyrReg(LSM9DS1_ORIENT_CFG_G,0b10111000); // Swap orientation
// Enable the accelerometer
writeAccReg(LSM9DS1_CTRL_REG5_XL,0b00111000); // z, y, x axis enabled for accelerometer
writeAccReg(LSM9DS1_CTRL_REG6_XL,0b00101000); // +/- 16g
//Enable the magnetometer
writeMagReg(LSM9DS1_CTRL_REG1_M, 0b10011100); // Temp compensation enabled,Low power mode mode,80Hz ODR
writeMagReg(LSM9DS1_CTRL_REG2_M, 0b01000000); // +/-12gauss
writeMagReg(LSM9DS1_CTRL_REG3_M, 0b00000000); // continuos update
writeMagReg(LSM9DS1_CTRL_REG4_M, 0b00000000); // lower power mode for Z axis
}
}
