Toshihisa T
/
LSM303DLHTest
LSM303DLH Test Program for get angle.
Diff: main.cpp
- Revision:
- 3:f3796683b4c9
- Parent:
- 2:39cdefe3c9f2
- Child:
- 4:3c677edffb13
--- a/main.cpp Tue Nov 30 14:42:07 2010 +0000 +++ b/main.cpp Sun May 06 05:14:30 2012 +0000 @@ -1,122 +1,26 @@ - -/* - * LSM303DLH Test program - * @tosihisa - */ #include "mbed.h" -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -Serial debug(USBTX,USBRX); - -I2C LSM303DLH(p28, p27); - -const int addr_acc = 0x30; -const int addr_mag = 0x3c; - -enum REG_ADDRS { - /* --- Mag --- */ - CRA_REG_M = 0x00, - CRB_REG_M = 0x01, - MR_REG_M = 0x02, - OUT_X_M = 0x03, - OUT_Y_M = 0x05, - OUT_Z_M = 0x07, - /* --- Acc --- */ - CTRL_REG1_A = 0x20, - CTRL_REG4_A = 0x23, - OUT_X_A = 0x28, - OUT_Y_A = 0x2A, - OUT_Z_A = 0x2C, -}; + #include "LSM303DLH.h" -bool write_reg(int addr_i2c,int addr_reg, char v) -{ - char data[2] = {addr_reg, v}; - return LSM303DLH.write(addr_i2c, data, 2) == 0; -} - -bool read_reg(int addr_i2c,int addr_reg, char *v) -{ - char data = addr_reg; - if ((LSM303DLH.write(addr_i2c, &data, 1) == 0) && (LSM303DLH.read(addr_i2c, &data, 1) == 0)){ - *v = data; - return true; - } - return false; -} - -bool read_reg_short(int addr_i2c,int addr_reg, short *v) -{ - char *pv = (char *)v; - read_reg(addr_i2c,addr_reg+0,pv+1); - return read_reg(addr_i2c,addr_reg+1,pv+0); -} - -int main() { - char reg_v; - bool result; - short a_x,a_y,a_z; - short m_x,m_y,m_z; - double m_deg; + Serial debug(USBTX,USBRX); + LSM303DLH compass(p28, p27); - debug.format(8,Serial::None,1); - debug.baud(115200); - debug.printf("LSM303DLH Test\x0d\x0a"); - - /* --- acc --- */ - reg_v = 0; - result = read_reg(addr_acc,CTRL_REG1_A,®_v); - debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v); - - reg_v = 0; - result = read_reg(addr_mag,CRA_REG_M,®_v); - debug.printf("(%s) CRA_REG_M=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v); - - reg_v = 0; - reg_v |= 0x01 << 5; /* Normal mode */ - reg_v |= 0x07; /* X/Y/Z axis enable. */ - result = write_reg(addr_acc,CTRL_REG1_A,reg_v); - reg_v = 0; - result = read_reg(addr_acc,CTRL_REG1_A,®_v); - debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v); - - reg_v = 0; - reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */ - reg_v |= 0x01 << 4; /* +/- 4g */ - result = write_reg(addr_acc,CTRL_REG4_A,reg_v); - - /* -- mag --- */ - reg_v = 0; - reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */ - result = write_reg(addr_mag,CRA_REG_M,reg_v); - - reg_v = 0; - //reg_v |= 0x01 << 5; /* +-1.3Gauss */ - reg_v |= 0x07 << 5; /* +-8.1Gauss */ - result = write_reg(addr_mag,CRB_REG_M,reg_v); - - reg_v = 0; /* Continuous-conversion mode */ - 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; - if(m_deg < 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); - debug.printf("MAG:X=%-7d,Y=%-7d,Z=%-7d DEG=%f\x0d\x0a",m_x,m_y,m_z,m_deg); - wait(0.1); - } -} + int main() { + float hdg; + float hdgV; + vector acc; + vector mag; + debug.format(8,Serial::None,1); + debug.baud(115200); + debug.printf("LSM303DLH Test\x0d\x0a"); + compass.setOffset(0.00,0.00,0.00); // example calibration + compass.setScale(1.00,1.00,1.00); // example calibration + while(1) { + compass.read(acc,mag); + hdg = compass.heading(); + hdgV = atan2(acc.y,acc.z) * 180/M_PI; + debug.printf("ACC: %6.2f %6.2f %6.2f Heading: %6.2f %6.2f\n",acc.x,acc.y,acc.z,hdgV,hdg); + wait(0.1); + } + } + \ No newline at end of file