Curtis Belknap / Mbed 2 deprecated LSM303DLHTest

LSM303DLH Test Program for get angle.

Dependencies:   mbed

Fork of LSM303DLHTest by Toshihisa T

main.cpp@1:14b97953bd2f, 2010-11-28 (annotated)

Committer:
tosihisa
Date:
Sun Nov 28 14:38:44 2010 +0000
Revision:
1:14b97953bd2f
Parent:
0:750784997b84
Child:
2:39cdefe3c9f2
magnetometer bug-fix.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tosihisa 0:750784997b84 1
tosihisa 0:750784997b84 2 /*
tosihisa 0:750784997b84 3 * LSM303DLH Test program
tosihisa 0:750784997b84 4 * @tosihisa
tosihisa 0:750784997b84 5 */
tosihisa 0:750784997b84 6
tosihisa 0:750784997b84 7 #include "mbed.h"
tosihisa 0:750784997b84 8 #ifndef M_PI
tosihisa 0:750784997b84 9 #define M_PI 3.14159265358979323846
tosihisa 0:750784997b84 10 #endif
tosihisa 0:750784997b84 11
tosihisa 0:750784997b84 12 Serial debug(USBTX,USBRX);
tosihisa 0:750784997b84 13
tosihisa 0:750784997b84 14 I2C LSM303DLH(p28, p27);
tosihisa 0:750784997b84 15
tosihisa 0:750784997b84 16 const int addr_acc = 0x30;
tosihisa 0:750784997b84 17 const int addr_mag = 0x3c;
tosihisa 0:750784997b84 18
tosihisa 0:750784997b84 19 enum REG_ADDRS {
tosihisa 0:750784997b84 20 /* --- Mag --- */
tosihisa 0:750784997b84 21 CRA_REG_M = 0x00,
tosihisa 0:750784997b84 22 CRB_REG_M = 0x01,
tosihisa 0:750784997b84 23 MR_REG_M = 0x02,
tosihisa 0:750784997b84 24 OUT_X_M = 0x03,
tosihisa 0:750784997b84 25 OUT_Y_M = 0x05,
tosihisa 0:750784997b84 26 OUT_Z_M = 0x07,
tosihisa 0:750784997b84 27 /* --- Acc --- */
tosihisa 0:750784997b84 28 CTRL_REG1_A = 0x20,
tosihisa 0:750784997b84 29 OUT_X_A = 0x28,
tosihisa 0:750784997b84 30 OUT_Y_A = 0x2A,
tosihisa 0:750784997b84 31 OUT_Z_A = 0x2C,
tosihisa 0:750784997b84 32 };
tosihisa 0:750784997b84 33
tosihisa 0:750784997b84 34 bool write_reg(int addr_i2c,int addr_reg, char v)
tosihisa 0:750784997b84 35 {
tosihisa 0:750784997b84 36 char data[2] = {addr_reg, v};
tosihisa 0:750784997b84 37 return LSM303DLH.write(addr_i2c, data, 2) == 0;
tosihisa 0:750784997b84 38 }
tosihisa 0:750784997b84 39
tosihisa 0:750784997b84 40 bool read_reg(int addr_i2c,int addr_reg, char *v)
tosihisa 0:750784997b84 41 {
tosihisa 0:750784997b84 42 char data = addr_reg;
tosihisa 0:750784997b84 43 if ((LSM303DLH.write(addr_i2c, &data, 1) == 0) && (LSM303DLH.read(addr_i2c, &data, 1) == 0)){
tosihisa 0:750784997b84 44 *v = data;
tosihisa 0:750784997b84 45 return true;
tosihisa 0:750784997b84 46 }
tosihisa 0:750784997b84 47 return false;
tosihisa 0:750784997b84 48 }
tosihisa 0:750784997b84 49
tosihisa 1:14b97953bd2f 50 bool read_reg_short(int addr_i2c,int addr_reg, short *v,int hi)
tosihisa 0:750784997b84 51 {
tosihisa 0:750784997b84 52 char *pv = (char *)v;
tosihisa 1:14b97953bd2f 53 if(hi){
tosihisa 1:14b97953bd2f 54 read_reg(addr_i2c,addr_reg+0,pv+1);
tosihisa 1:14b97953bd2f 55 return read_reg(addr_i2c,addr_reg+1,pv+0);
tosihisa 1:14b97953bd2f 56 } else {
tosihisa 1:14b97953bd2f 57 read_reg(addr_i2c,addr_reg+0,pv+0);
tosihisa 1:14b97953bd2f 58 return read_reg(addr_i2c,addr_reg+1,pv+1);
tosihisa 1:14b97953bd2f 59 }
tosihisa 0:750784997b84 60 }
tosihisa 0:750784997b84 61
tosihisa 0:750784997b84 62 int main() {
tosihisa 0:750784997b84 63 char reg_v;
tosihisa 0:750784997b84 64 bool result;
tosihisa 0:750784997b84 65 short a_x,a_y,a_z;
tosihisa 0:750784997b84 66 short m_x,m_y,m_z;
tosihisa 0:750784997b84 67 double m_deg;
tosihisa 0:750784997b84 68
tosihisa 0:750784997b84 69 debug.format(8,Serial::None,1);
tosihisa 0:750784997b84 70 debug.baud(115200);
tosihisa 0:750784997b84 71 debug.printf("LSM303DLH Test\x0d\x0a");
tosihisa 0:750784997b84 72
tosihisa 0:750784997b84 73 reg_v = 0;
tosihisa 0:750784997b84 74 result = read_reg(addr_acc,CTRL_REG1_A,&reg_v);
tosihisa 0:750784997b84 75 debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
tosihisa 0:750784997b84 76
tosihisa 0:750784997b84 77 reg_v = 0;
tosihisa 0:750784997b84 78 result = read_reg(addr_mag,CRA_REG_M,&reg_v);
tosihisa 0:750784997b84 79 debug.printf("(%s) CRA_REG_M=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
tosihisa 0:750784997b84 80
tosihisa 0:750784997b84 81 reg_v = 0;
tosihisa 0:750784997b84 82 reg_v |= 0x01 << 5; /* Normal mode */
tosihisa 0:750784997b84 83 reg_v |= 0x07; /* X/Y/Z axis enable. */
tosihisa 0:750784997b84 84 result = write_reg(addr_acc,CTRL_REG1_A,reg_v);
tosihisa 0:750784997b84 85 reg_v = 0;
tosihisa 0:750784997b84 86 result = read_reg(addr_acc,CTRL_REG1_A,&reg_v);
tosihisa 0:750784997b84 87 debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
tosihisa 0:750784997b84 88
tosihisa 0:750784997b84 89 reg_v = 0;
tosihisa 0:750784997b84 90 reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */
tosihisa 0:750784997b84 91 result = write_reg(addr_mag,CRA_REG_M,reg_v);
tosihisa 0:750784997b84 92
tosihisa 0:750784997b84 93 reg_v = 0;
tosihisa 0:750784997b84 94 reg_v |= 0x01 << 5; /* +-1.3Gauss */
tosihisa 0:750784997b84 95 result = write_reg(addr_mag,CRB_REG_M,reg_v);
tosihisa 0:750784997b84 96
tosihisa 0:750784997b84 97 reg_v = 0; /* Continuous-conversion mode */
tosihisa 0:750784997b84 98 result = write_reg(addr_mag,MR_REG_M,reg_v);
tosihisa 0:750784997b84 99
tosihisa 0:750784997b84 100 while(1) {
tosihisa 1:14b97953bd2f 101 read_reg_short(addr_acc,OUT_X_A,&a_x,0);
tosihisa 1:14b97953bd2f 102 read_reg_short(addr_acc,OUT_Y_A,&a_y,0);
tosihisa 1:14b97953bd2f 103 read_reg_short(addr_acc,OUT_Z_A,&a_z,0);
tosihisa 1:14b97953bd2f 104 read_reg_short(addr_mag,OUT_X_M,&m_x,1);
tosihisa 1:14b97953bd2f 105 read_reg_short(addr_mag,OUT_Y_M,&m_y,1);
tosihisa 1:14b97953bd2f 106 read_reg_short(addr_mag,OUT_Z_M,&m_z,1);
tosihisa 1:14b97953bd2f 107 m_deg = (atan2((double)(m_x),(double)(m_y)) * 180) / M_PI;
tosihisa 0:750784997b84 108 if(m_deg < 0){
tosihisa 1:14b97953bd2f 109 m_deg = m_deg * -1.0;
tosihisa 1:14b97953bd2f 110 } else {
tosihisa 1:14b97953bd2f 111 m_deg = 360.0 - m_deg;
tosihisa 0:750784997b84 112 }
tosihisa 0:750784997b84 113
tosihisa 0:750784997b84 114 debug.printf("ACC:X=%-7d,Y=%-7d,Z=%-7d ",a_x,a_y,a_z);
tosihisa 0:750784997b84 115 debug.printf("MAG:X=%-7d,Y=%-7d,Z=%-7d DEG=%f\x0d\x0a",m_x,m_y,m_z,m_deg);
tosihisa 0:750784997b84 116 wait(0.1);
tosihisa 0:750784997b84 117 }
tosihisa 0:750784997b84 118 }