Curtis Belknap / Mbed 2 deprecated LSM303DLHTest

LSM303DLH Test Program for get angle.

Dependencies:   mbed

Fork of LSM303DLHTest by Toshihisa T

main.cpp@0:750784997b84, 2010-11-28 (annotated)

Committer:
tosihisa
Date:
Sun Nov 28 13:29:02 2010 +0000
Revision:
0:750784997b84
Child:
1:14b97953bd2f
1st Release

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 0:750784997b84 50 bool read_reg_short(int addr_i2c,int addr_reg, short *v)
tosihisa 0:750784997b84 51 {
tosihisa 0:750784997b84 52 char *pv = (char *)v;
tosihisa 0:750784997b84 53 read_reg(addr_i2c,addr_reg+0,pv+0);
tosihisa 0:750784997b84 54 return read_reg(addr_i2c,addr_reg+1,pv+1);
tosihisa 0:750784997b84 55 }
tosihisa 0:750784997b84 56
tosihisa 0:750784997b84 57 int main() {
tosihisa 0:750784997b84 58 char reg_v;
tosihisa 0:750784997b84 59 bool result;
tosihisa 0:750784997b84 60 short a_x,a_y,a_z;
tosihisa 0:750784997b84 61 short m_x,m_y,m_z;
tosihisa 0:750784997b84 62 double m_deg;
tosihisa 0:750784997b84 63
tosihisa 0:750784997b84 64 debug.format(8,Serial::None,1);
tosihisa 0:750784997b84 65 debug.baud(115200);
tosihisa 0:750784997b84 66 debug.printf("LSM303DLH Test\x0d\x0a");
tosihisa 0:750784997b84 67
tosihisa 0:750784997b84 68 reg_v = 0;
tosihisa 0:750784997b84 69 result = read_reg(addr_acc,CTRL_REG1_A,&reg_v);
tosihisa 0:750784997b84 70 debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
tosihisa 0:750784997b84 71
tosihisa 0:750784997b84 72 reg_v = 0;
tosihisa 0:750784997b84 73 result = read_reg(addr_mag,CRA_REG_M,&reg_v);
tosihisa 0:750784997b84 74 debug.printf("(%s) CRA_REG_M=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
tosihisa 0:750784997b84 75
tosihisa 0:750784997b84 76 reg_v = 0;
tosihisa 0:750784997b84 77 reg_v |= 0x01 << 5; /* Normal mode */
tosihisa 0:750784997b84 78 reg_v |= 0x07; /* X/Y/Z axis enable. */
tosihisa 0:750784997b84 79 result = write_reg(addr_acc,CTRL_REG1_A,reg_v);
tosihisa 0:750784997b84 80 reg_v = 0;
tosihisa 0:750784997b84 81 result = read_reg(addr_acc,CTRL_REG1_A,&reg_v);
tosihisa 0:750784997b84 82 debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
tosihisa 0:750784997b84 83
tosihisa 0:750784997b84 84 reg_v = 0;
tosihisa 0:750784997b84 85 reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */
tosihisa 0:750784997b84 86 result = write_reg(addr_mag,CRA_REG_M,reg_v);
tosihisa 0:750784997b84 87
tosihisa 0:750784997b84 88 reg_v = 0;
tosihisa 0:750784997b84 89 reg_v |= 0x01 << 5; /* +-1.3Gauss */
tosihisa 0:750784997b84 90 result = write_reg(addr_mag,CRB_REG_M,reg_v);
tosihisa 0:750784997b84 91
tosihisa 0:750784997b84 92 reg_v = 0; /* Continuous-conversion mode */
tosihisa 0:750784997b84 93 result = write_reg(addr_mag,MR_REG_M,reg_v);
tosihisa 0:750784997b84 94
tosihisa 0:750784997b84 95 while(1) {
tosihisa 0:750784997b84 96 read_reg_short(addr_acc,OUT_X_A,&a_x);
tosihisa 0:750784997b84 97 read_reg_short(addr_acc,OUT_Y_A,&a_y);
tosihisa 0:750784997b84 98 read_reg_short(addr_acc,OUT_Z_A,&a_z);
tosihisa 0:750784997b84 99 read_reg_short(addr_mag,OUT_X_M,&m_x);
tosihisa 0:750784997b84 100 read_reg_short(addr_mag,OUT_Y_M,&m_y);
tosihisa 0:750784997b84 101 read_reg_short(addr_mag,OUT_Z_M,&m_z);
tosihisa 0:750784997b84 102 m_deg = (atan2((double)m_x,(double)m_y) * 180) / M_PI;
tosihisa 0:750784997b84 103 if(m_deg < 0){
tosihisa 0:750784997b84 104 m_deg += 360.0;
tosihisa 0:750784997b84 105 }
tosihisa 0:750784997b84 106
tosihisa 0:750784997b84 107 debug.printf("ACC:X=%-7d,Y=%-7d,Z=%-7d ",a_x,a_y,a_z);
tosihisa 0:750784997b84 108 debug.printf("MAG:X=%-7d,Y=%-7d,Z=%-7d DEG=%f\x0d\x0a",m_x,m_y,m_z,m_deg);
tosihisa 0:750784997b84 109 wait(0.1);
tosihisa 0:750784997b84 110 }
tosihisa 0:750784997b84 111 }