LSM303DLH Test Program for get angle.
Dependencies: L3GD20 LSM303DLHC SDFileSystemLatch mbed nrf51_rtc
Fork of LSM303DLHTest by
Diff: main.cpp
- Revision:
- 1:14b97953bd2f
- Parent:
- 0:750784997b84
- Child:
- 2:39cdefe3c9f2
diff -r 750784997b84 -r 14b97953bd2f main.cpp --- a/main.cpp Sun Nov 28 13:29:02 2010 +0000 +++ b/main.cpp Sun Nov 28 14:38:44 2010 +0000 @@ -47,11 +47,16 @@ return false; } -bool read_reg_short(int addr_i2c,int addr_reg, short *v) +bool read_reg_short(int addr_i2c,int addr_reg, short *v,int hi) { char *pv = (char *)v; - read_reg(addr_i2c,addr_reg+0,pv+0); - return read_reg(addr_i2c,addr_reg+1,pv+1); + if(hi){ + read_reg(addr_i2c,addr_reg+0,pv+1); + return read_reg(addr_i2c,addr_reg+1,pv+0); + } else { + read_reg(addr_i2c,addr_reg+0,pv+0); + return read_reg(addr_i2c,addr_reg+1,pv+1); + } } int main() { @@ -93,15 +98,17 @@ result = write_reg(addr_mag,MR_REG_M,reg_v); while(1) { - read_reg_short(addr_acc,OUT_X_A,&a_x); - read_reg_short(addr_acc,OUT_Y_A,&a_y); - read_reg_short(addr_acc,OUT_Z_A,&a_z); - read_reg_short(addr_mag,OUT_X_M,&m_x); - read_reg_short(addr_mag,OUT_Y_M,&m_y); - read_reg_short(addr_mag,OUT_Z_M,&m_z); - m_deg = (atan2((double)m_x,(double)m_y) * 180) / M_PI; + read_reg_short(addr_acc,OUT_X_A,&a_x,0); + read_reg_short(addr_acc,OUT_Y_A,&a_y,0); + read_reg_short(addr_acc,OUT_Z_A,&a_z,0); + read_reg_short(addr_mag,OUT_X_M,&m_x,1); + read_reg_short(addr_mag,OUT_Y_M,&m_y,1); + read_reg_short(addr_mag,OUT_Z_M,&m_z,1); + m_deg = (atan2((double)(m_x),(double)(m_y)) * 180) / M_PI; if(m_deg < 0){ - m_deg += 360.0; + m_deg = m_deg * -1.0; + } else { + m_deg = 360.0 - m_deg; } debug.printf("ACC:X=%-7d,Y=%-7d,Z=%-7d ",a_x,a_y,a_z);