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