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()
{
__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
}
// Turns on the LSM303's accelerometer and magnetometers and places them in normal
// mode.
void LSM303::enableDefault(void)
{
// Enable Accelerometer
// 0x27 = 0b00100111
// Normal power mode, all axes enabled
writeAccReg(LSM303_CTRL_REG1_A, 0x27);
if (_device == LSM303DLHC_DEVICE)
writeAccReg(LSM303_CTRL_REG4_A, 0x20);//0x08); // DLHC: enable high resolution mode //+/-8g 4mg/LSB
// Enable Magnetometer
// 0x00 = 0b00000000
// Continuous conversion mode
writeMagReg(LSM303_MR_REG_M, 0x00);
}
void mag_init()
{
i2c_init();
_delay_ms(10);
writeAccReg(0x20, 0x27);
writeMagReg(0x02, 0);
writeMagReg(0x00, 0b00011000);
}
// Turns on the LSM303DLH's accelerometer and magnetometers and places them in normal
// mode.
void LSM303DLH::enableDefault(void)
{
// Enable Accelerometer
// 0x27 = 0b00100111
// Normal power mode, all axes enabled
writeAccReg(LSM303DLH_CTRL_REG1_A, 0x27);
// Enable Magnetometer
// 0x00 = 0b00000000
// Continuous conversion mode
writeMagReg(LSM303DLH_MR_REG_M, 0x00);
}
void LSM303::writeReg(byte reg, byte value)
{
// mag address == acc_address for LSM303D, so it doesn't really matter which one we use.
if (_device == device_D || reg < CTRL_REG1_A)
{
writeMagReg(reg, value);
}
else
{
writeAccReg(reg, value);
}
}
void LSM303::writeReg(regAddr reg, byte value)
{
// mag address == acc_address for LSM303D, so it doesn't really matter which one we use.
// Use writeMagReg so it can translate OUT_[XYZ]_[HL]_M
if (_device == device_D || reg < CTRL_REG1_A)
{
writeMagReg(reg, value);
}
else
{
writeAccReg(reg, value);
}
}
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
}
}
/*
Enables the LSM303's accelerometer and magnetometer. Also:
- Sets sensor full scales (gain) to default power-on values, which are
+/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer
(+/- 4 gauss on LSM303D).
- Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz
ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR
settings for which the electrical characteristics are specified in
the datasheets.)
- Enables high resolution modes (if available).
Note that this function will also reset other settings controlled by
the registers it writes to.
*/
void LSM303::enableDefault(void)
{
if (_device == device_D)
{
// Accelerometer
// 0x57 = 0b01010111
// AFS = 0 (+/- 2 g full scale)
// 0b 00 (362 Hz anti-alias BW), [000:+/- 2g, 001: +/- 4g, 010: +/- 6g, 011: +/- 8g, 100: +/- 16g AFS],
// 0, 0 (self test disabled), 0 (4 wire interface)
// 0x08:+/- 4g, 0x10: +/- 6g, 0x18: +/- 8g, 0x20: +/- 16g
writeReg(CTRL2, 0x00);
// 0x57 = 0b01010111
// AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) - 0x57
// AODR = 0110 (100 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) - MJS CHANGE
writeReg(CTRL1, 0x57);
// Magnetometer
// 0x64 = 0b01100100
// M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
// M_RES = 11 (high resolution mode); M_ODR = 101 (100 Hz ODR only available for accel >50hz) MJS
//writeReg(CTRL5, 0x64);
writeReg(CTRL5, 0x64);
// 0x20 = 0b00100000
// MFS = 01 (+/- 4 gauss full scale) - MFS = 00(+/- 2 gauss full scale)
// MFS = 10 (+/- 8 gauss full scale) - MFS = 11(+/- 12 gauss full scale)
// original default was 01 (changed MJS - 5/13/14)
writeReg(CTRL6, 0x00);
// 0x00 = 0b00000000
// MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
writeReg(CTRL7, 0x00);
}
else if (_device == device_DLHC)
{
// Accelerometer
// 0x08 = 0b00001000
// FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable)
// FS = 0x10 (+/- 4 g), 0x20: +/- 2 g, 0x30: +/- 16 g
writeAccReg(CTRL_REG4_A, 0x08);
// 0x47 = 0b01000111
// ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
// ODR = 0110 (100 Hz 0DR)
writeAccReg(CTRL_REG1_A, 0x57);
// Magnetometer
// 0x0C = 0b00001100
// DO = 011 (7.5 Hz ODR)
// DO = 110 (75 Hz ODR), 0x18
writeMagReg(CRA_REG_M, 0x0C);
// 0x20 = 0b00100000
// GN = 001 (+/- 1.3 gauss full scale)
//
// GN2 GN1 GN0 Sensor input
// field range Gain X, Y, and Z Gain Z Output range
// [Gauss] [LSB/Gauss] [LSB/Gauss]
// 0 0 1 ±1.3 1100 980
// 0 1 0 ±1.9 855 760
// 0 1 1 ±2.5 670 600 0xF800–0x07FF
// 1 0 0 ±4.0 450 400 (-2048 to +2047)
// 1 0 1 ±4.7 400 355
// 1 1 0 ±5.6 330 295
// 1 1 1 ±8.1 230 205
writeMagReg(CRB_REG_M, 0x20);
// 0x00 = 0b00000000
// MD = 00 (continuous-conversion mode)
writeMagReg(MR_REG_M, 0x00);
}
else // DLM, DLH
{
// Accelerometer
// 0x00 = 0b00000000
// FS = 00 (+/- 2 g full scale)
writeAccReg(CTRL_REG4_A, 0x00);
// 0x27 = 0b00100111
// PM = 001 (normal mode); DR = 00 (50 Hz ODR); Zen = Yen = Xen = 1 (all axes enabled)
writeAccReg(CTRL_REG1_A, 0x27);
// Magnetometer
// 0x0C = 0b00001100
// DO = 011 (7.5 Hz ODR)
writeMagReg(CRA_REG_M, 0x0C);
// 0x20 = 0b00100000
// GN = 001 (+/- 1.3 gauss full scale)
writeMagReg(CRB_REG_M, 0x20);
// 0x00 = 0b00000000
// MD = 00 (continuous-conversion mode)
writeMagReg(MR_REG_M, 0x00);
}
}
// Enable accelerometer.
void enableACC()
{
writeAccReg(LSM303_CTRL_REG1_A, 0b01010111); // z,y,x axis enabled , 100Hz data rate
writeAccReg(LSM303_CTRL_REG4_A, 0b00101000); // +/- 8G full scale: FS = 10 on DLHC, high resolution output mode
}
int main(int argc, char *argv[])
{
char filename[20];
int magRaw[3];
int accRaw[3];
float accXnorm,accYnorm,pitch,roll,magXcomp,magYcomp;
//Open the i2c bus
sprintf(filename, "/dev/i2c-%d", 1);
file = open(filename, O_RDWR);
if (file<0) {
printf("Unable to open I2C bus!");
exit(1);
}
//Select the magnetomoter
if (ioctl(file, I2C_SLAVE, MAG_ADDRESS) < 0) {
printf("Error: Could not select magnetometer\n");
}
// 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
while(1)
{
readMAG(magRaw);
readACC(accRaw);
//If your IMU is upside down, comment out the two lines below which we correct the tilt calculation
//accRaw[0] = -accRaw[0];
//accRaw[1] = -accRaw[1];
//Compute heading
float heading = 180 * atan2(magRaw[1],magRaw[0])/M_PI;
//Convert heading to 0 - 360
if(heading < 0)
heading += 360;
printf("heading %7.3f \t ", heading);
//Normalize accelerometer raw values.
accXnorm = accRaw[0]/sqrt(accRaw[0] * accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);
accYnorm = accRaw[1]/sqrt(accRaw[0] * accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);
//Calculate pitch and roll
pitch = asin(accXnorm);
roll = -asin(accYnorm/cos(pitch));
//Calculate the new tilt compensated values
magXcomp = magRaw[0]*cos(pitch)+magRaw[02]*sin(pitch);
magYcomp = magRaw[0]*sin(roll)*sin(pitch)+magRaw[1]*cos(roll)-magRaw[2]*sin(roll)*cos(pitch);
//Calculate heading
heading = 180*atan2(magYcomp,magXcomp)/M_PI;
//Convert heading to 0 - 360
if(heading < 0)
heading += 360;
printf("Compensated Heading %7.3f \n", heading);
//Sleep for 25ms
usleep(25000);
}
}
/*
Enables the LSM303's accelerometer and magnetometer. Also:
- Sets sensor full scales (gain) to default power-on values, which are
+/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer
(+/- 4 gauss on LSM303D).
- Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz
ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR
settings for which the electrical characteristics are specified in
the datasheets.)
- Enables high resolution modes (if available).
Note that this function will also reset other settings controlled by
the registers it writes to.
*/
void LSM303::enableDefault(void)
{
if (_device == device_D)
{
// Accelerometer
// 0x00 = 0b00000000
// AFS = 0 (+/- 2 g full scale)
writeReg(CTRL2, 0x00);
// 0x57 = 0b01010111
// AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
writeReg(CTRL1, 0x57);
// Magnetometer
// 0x64 = 0b01100100
// M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
writeReg(CTRL5, 0x64);
// 0x20 = 0b00100000
// MFS = 01 (+/- 4 gauss full scale)
writeReg(CTRL6, 0x20);
// 0x00 = 0b00000000
// MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
writeReg(CTRL7, 0x00);
}
else
{
// Accelerometer
if (_device == device_DLHC)
{
// 0x08 = 0b00001000
// FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable)
writeAccReg(CTRL_REG4_A, 0x08);
// 0x47 = 0b01000111
// ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
writeAccReg(CTRL_REG1_A, 0x47);
}
else // DLM, DLH
{
// 0x00 = 0b00000000
// FS = 00 (+/- 2 g full scale)
writeAccReg(CTRL_REG4_A, 0x00);
// 0x27 = 0b00100111
// PM = 001 (normal mode); DR = 00 (50 Hz ODR); Zen = Yen = Xen = 1 (all axes enabled)
writeAccReg(CTRL_REG1_A, 0x27);
}
// Magnetometer
// 0x0C = 0b00001100
// DO = 011 (7.5 Hz ODR)
writeMagReg(CRA_REG_M, 0x0C);
// 0x20 = 0b00100000
// GN = 001 (+/- 1.3 gauss full scale)
writeMagReg(CRB_REG_M, 0x20);
// 0x00 = 0b00000000
// MD = 00 (continuous-conversion mode)
writeMagReg(MR_REG_M, 0x00);
}
}