void LSM303::init(byte device, byte sa0_a)
{
_device = device;
switch (_device)
{
case LSM303DLH_DEVICE:
case LSM303DLM_DEVICE:
if (sa0_a == LSM303_SA0_A_LOW)
acc_address = ACC_ADDRESS_SA0_A_LOW;
else if (sa0_a == LSM303_SA0_A_HIGH)
acc_address = ACC_ADDRESS_SA0_A_HIGH;
else
acc_address = (detectSA0_A() == LSM303_SA0_A_HIGH) ? ACC_ADDRESS_SA0_A_HIGH : ACC_ADDRESS_SA0_A_LOW;
break;
case LSM303DLHC_DEVICE:
acc_address = ACC_ADDRESS_SA0_A_HIGH;
break;
default:
// try to auto-detect device
if (detectSA0_A() == LSM303_SA0_A_HIGH)
{
// if device responds on 0011001b (SA0_A is high), assume DLHC
acc_address = ACC_ADDRESS_SA0_A_HIGH;
_device = LSM303DLHC_DEVICE;
}
else
{
// otherwise, assume DLH or DLM (pulled low by default on Pololu boards); query magnetometer WHO_AM_I to differentiate these two
acc_address = ACC_ADDRESS_SA0_A_LOW;
_device = (readMagReg(LSM303_WHO_AM_I_M) == 0x3C) ? LSM303DLM_DEVICE : LSM303DLH_DEVICE;
}
}
}
// Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC.
// To read those two registers, use readMagReg() instead.
byte LSM303::readReg(regAddr reg)
{
// 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)
{
return readMagReg(reg);
}
else
{
return readAccReg(reg);
}
}
int read_compass()
{
ax = (double)(((int)readAccReg(0x29) << 8 | readAccReg(0x28)) /
256);
ay = (double)(((int)readAccReg(0x2b) << 8 | readAccReg(0x2a)) /
256);
az = (double)(((int)readAccReg(0x2d) << 8 | readAccReg(0x2c)) /
256);
mx = (double)((int16_t) readMagReg(0x3) << 8 | readMagReg(0x4));
mz = (double)((int16_t) readMagReg(0x5) << 8 | readMagReg(0x6));
my = (double)((int16_t) readMagReg(0x7) << 8 | readMagReg(0x8));
mx = ((mx - minx) / (maxx - minx)) * 2 - 1.0;
my = ((my - miny) / (maxy - miny)) * 2 - 1.0;
mz = ((mz - minz) / (maxz - minz)) * 2 - 1.0;
ACC = sqrt((ax * ax) + (ay * ay) + (az * az));
pitch = asin(-ax / ACC);
roll = asin((ay / ACC) / cos(pitch));
mx2 = mx * cos(pitch) + mz * sin(pitch);
my2 =
mx * sin(pitch) * sin(roll) + my * cos(roll) -
mz * sin(roll) * cos(pitch);
if (mx2 > 0 && my2 >= 0) {
return ((int)(atan(my2 / mx2) * 180 / pi));
//uart_print_num((int)(atan(my / mx) * 180 / pi));
} else if (mx2 < 0) {
return (180 + (int)(atan(my2 / mx2) * 180 / pi));
//uart_print_num(180 + (int)(atan(my / mx) * 180 / pi));
} else if (mx2 > 0 && my2 <= 0) {
return (360 + (int)(atan(my2 / mx2) * 180 / pi));
//uart_print_num(360 + (int)(atan(my / mx) * 180 / pi));
} else if (mx2 == 0 && my2 < 0) {
return (90);
//uart_print_num(90);
} else if (mx2 == 0 && my2 > 0) {
return (270);
//uart_print_num(270);
}
}
