Diff: main.cpp

Revision:

0:750784997b84

Child:

1:14b97953bd2f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Nov 28 13:29:02 2010 +0000
@@ -0,0 +1,111 @@
+
+/*
+ * 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,
+    OUT_X_A     = 0x28,
+    OUT_Y_A     = 0x2A,
+    OUT_Z_A     = 0x2C,
+};
+
+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+0);
+    return read_reg(addr_i2c,addr_reg+1,pv+1);
+}
+
+int main() {
+    char reg_v;
+    bool result;
+    short a_x,a_y,a_z;
+    short m_x,m_y,m_z;
+    double m_deg;
+
+    debug.format(8,Serial::None,1);
+    debug.baud(115200);
+    debug.printf("LSM303DLH Test\x0d\x0a");
+
+    reg_v = 0;
+    result = read_reg(addr_acc,CTRL_REG1_A,&reg_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,&reg_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,&reg_v);
+    debug.printf("(%s) CTRL_REG1_A=[0x%02X]\x0d\x0a",(result) ? "OK" : "NG",reg_v);
+
+    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 */
+    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 += 360.0;
+        }
+
+        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);
+    }
+}