Curtis Belknap / Mbed 2 deprecated LSM303DLHTest

LSM303DLH Test Program for get angle.

Dependencies:   mbed

Fork of LSM303DLHTest by Toshihisa T

main.cpp@2:39cdefe3c9f2, 2010-11-30 (annotated)

Committer:
tosihisa
Date:
Tue Nov 30 14:42:07 2010 +0000
Revision:
2:39cdefe3c9f2
Parent:
1:14b97953bd2f
Child:
3:f3796683b4c9
test +/-4g,8.1gauss

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 2:39cdefe3c9f2 29 CTRL_REG4_A = 0x23,
tosihisa 0:750784997b84 30 OUT_X_A = 0x28,
tosihisa 0:750784997b84 31 OUT_Y_A = 0x2A,
tosihisa 0:750784997b84 32 OUT_Z_A = 0x2C,
tosihisa 0:750784997b84 33 };
tosihisa 0:750784997b84 34
tosihisa 0:750784997b84 35 bool write_reg(int addr_i2c,int addr_reg, char v)
tosihisa 0:750784997b84 36 {
tosihisa 0:750784997b84 37 char data[2] = {addr_reg, v};
tosihisa 0:750784997b84 38 return LSM303DLH.write(addr_i2c, data, 2) == 0;
tosihisa 0:750784997b84 39 }
tosihisa 0:750784997b84 40
tosihisa 0:750784997b84 41 bool read_reg(int addr_i2c,int addr_reg, char *v)
tosihisa 0:750784997b84 42 {
tosihisa 0:750784997b84 43 char data = addr_reg;
tosihisa 0:750784997b84 44 if ((LSM303DLH.write(addr_i2c, &data, 1) == 0) && (LSM303DLH.read(addr_i2c, &data, 1) == 0)){
tosihisa 0:750784997b84 45 *v = data;
tosihisa 0:750784997b84 46 return true;
tosihisa 0:750784997b84 47 }
tosihisa 0:750784997b84 48 return false;
tosihisa 0:750784997b84 49 }
tosihisa 0:750784997b84 50
tosihisa 2:39cdefe3c9f2 51 bool read_reg_short(int addr_i2c,int addr_reg, short *v)
tosihisa 0:750784997b84 52 {
tosihisa 0:750784997b84 53 char *pv = (char *)v;
tosihisa 2:39cdefe3c9f2 54 read_reg(addr_i2c,addr_reg+0,pv+1);
tosihisa 2:39cdefe3c9f2 55 return read_reg(addr_i2c,addr_reg+1,pv+0);
tosihisa 0:750784997b84 56 }
tosihisa 0:750784997b84 57
tosihisa 0:750784997b84 58 int main() {
tosihisa 0:750784997b84 59 char reg_v;
tosihisa 0:750784997b84 60 bool result;
tosihisa 0:750784997b84 61 short a_x,a_y,a_z;
tosihisa 0:750784997b84 62 short m_x,m_y,m_z;
tosihisa 0:750784997b84 63 double m_deg;
tosihisa 0:750784997b84 64
tosihisa 0:750784997b84 65 debug.format(8,Serial::None,1);
tosihisa 0:750784997b84 66 debug.baud(115200);
tosihisa 0:750784997b84 67 debug.printf("LSM303DLH Test\x0d\x0a");
tosihisa 0:750784997b84 68
tosihisa 2:39cdefe3c9f2 69 /* --- acc --- */
tosihisa 0:750784997b84 70 reg_v = 0;
tosihisa 0:750784997b84 71 result = read_reg(addr_acc,CTRL_REG1_A,&reg_v);
tosihisa 0:750784997b84 72 debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
tosihisa 0:750784997b84 73
tosihisa 0:750784997b84 74 reg_v = 0;
tosihisa 0:750784997b84 75 result = read_reg(addr_mag,CRA_REG_M,&reg_v);
tosihisa 0:750784997b84 76 debug.printf("(%s) CRA_REG_M=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
tosihisa 0:750784997b84 77
tosihisa 0:750784997b84 78 reg_v = 0;
tosihisa 0:750784997b84 79 reg_v |= 0x01 << 5; /* Normal mode */
tosihisa 0:750784997b84 80 reg_v |= 0x07; /* X/Y/Z axis enable. */
tosihisa 0:750784997b84 81 result = write_reg(addr_acc,CTRL_REG1_A,reg_v);
tosihisa 0:750784997b84 82 reg_v = 0;
tosihisa 0:750784997b84 83 result = read_reg(addr_acc,CTRL_REG1_A,&reg_v);
tosihisa 0:750784997b84 84 debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
tosihisa 0:750784997b84 85
tosihisa 0:750784997b84 86 reg_v = 0;
tosihisa 2:39cdefe3c9f2 87 reg_v |= 0x01 << 6; /* 1: data MSB @ lower address */
tosihisa 2:39cdefe3c9f2 88 reg_v |= 0x01 << 4; /* +/- 4g */
tosihisa 2:39cdefe3c9f2 89 result = write_reg(addr_acc,CTRL_REG4_A,reg_v);
tosihisa 2:39cdefe3c9f2 90
tosihisa 2:39cdefe3c9f2 91 /* -- mag --- */
tosihisa 2:39cdefe3c9f2 92 reg_v = 0;
tosihisa 0:750784997b84 93 reg_v |= 0x04 << 2; /* Minimum data output rate = 15Hz */
tosihisa 0:750784997b84 94 result = write_reg(addr_mag,CRA_REG_M,reg_v);
tosihisa 0:750784997b84 95
tosihisa 0:750784997b84 96 reg_v = 0;
tosihisa 2:39cdefe3c9f2 97 //reg_v |= 0x01 << 5; /* +-1.3Gauss */
tosihisa 2:39cdefe3c9f2 98 reg_v |= 0x07 << 5; /* +-8.1Gauss */
tosihisa 0:750784997b84 99 result = write_reg(addr_mag,CRB_REG_M,reg_v);
tosihisa 0:750784997b84 100
tosihisa 0:750784997b84 101 reg_v = 0; /* Continuous-conversion mode */
tosihisa 0:750784997b84 102 result = write_reg(addr_mag,MR_REG_M,reg_v);
tosihisa 0:750784997b84 103
tosihisa 0:750784997b84 104 while(1) {
tosihisa 2:39cdefe3c9f2 105 read_reg_short(addr_acc,OUT_X_A,&a_x);
tosihisa 2:39cdefe3c9f2 106 read_reg_short(addr_acc,OUT_Y_A,&a_y);
tosihisa 2:39cdefe3c9f2 107 read_reg_short(addr_acc,OUT_Z_A,&a_z);
tosihisa 2:39cdefe3c9f2 108 read_reg_short(addr_mag,OUT_X_M,&m_x);
tosihisa 2:39cdefe3c9f2 109 read_reg_short(addr_mag,OUT_Y_M,&m_y);
tosihisa 2:39cdefe3c9f2 110 read_reg_short(addr_mag,OUT_Z_M,&m_z);
tosihisa 1:14b97953bd2f 111 m_deg = (atan2((double)(m_x),(double)(m_y)) * 180) / M_PI;
tosihisa 0:750784997b84 112 if(m_deg < 0){
tosihisa 1:14b97953bd2f 113 m_deg = m_deg * -1.0;
tosihisa 1:14b97953bd2f 114 } else {
tosihisa 1:14b97953bd2f 115 m_deg = 360.0 - m_deg;
tosihisa 0:750784997b84 116 }
tosihisa 0:750784997b84 117
tosihisa 0:750784997b84 118 debug.printf("ACC:X=%-7d,Y=%-7d,Z=%-7d ",a_x,a_y,a_z);
tosihisa 0:750784997b84 119 debug.printf("MAG:X=%-7d,Y=%-7d,Z=%-7d DEG=%f\x0d\x0a",m_x,m_y,m_z,m_deg);
tosihisa 0:750784997b84 120 wait(0.1);
tosihisa 0:750784997b84 121 }
tosihisa 0:750784997b84 122 }