// 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);
}
}
