Diff: l3g.cpp

Revision:

40:6fa672be85ec

Child:

42:96671b71aac5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/l3g.cpp	Thu Jul 25 02:53:34 2019 +0000
@@ -0,0 +1,95 @@
+// Code for reading from the L3GD20H gyro with I2C.
+
+#include <mbed.h>
+#include "l3g.h"
+#include "pc_serial.h"
+
+I2C i2c(p9, p10);
+
+int address = 0xD6;
+
+int32_t l3gInit()
+{
+    int whoami = l3gReadReg(0x0F);
+    if (whoami != 0xD7)
+    {    
+        pc.printf("l3g whoami error: %d\n", whoami);
+        return -1;  // wrong ID or no response
+    }
+    
+    // CTRL1: 800 Hz output data rate,
+    // low-pass filter cutoff 100 Hz
+    l3gWriteReg(0x20, 0xFF);
+
+    // CTRL4: 2000 dps full scale
+    l3gWriteReg(0x23, 0x20);
+
+    // CTRL5: High-pass filter disabled
+    l3gWriteReg(0x24, 0x00);
+
+    return 0;  // success
+}
+
+int32_t l3gReadReg(char reg)
+{
+    int result = i2c.write(address, &reg, 1); 
+    if (result != 0)
+    {
+        return -1;  // error
+    }
+
+    char data;
+    result = i2c.read(address, &data, 1);
+    if (result != 0)
+    {
+        return -2;  // error
+    }
+  
+    return data;
+}
+
+int32_t l3gWriteReg(char reg, char value)
+{
+    char data[2];
+    data[0] = reg;
+    data[1] = value;
+    int result = i2c.write(address, data, 2); 
+    if (result != 0)
+    {
+        return -1;  // error
+    }
+    
+    return 0;
+}
+
+int32_t l3gZAvailable()
+{
+    // Read STATUS
+    int32_t result = l3gReadReg(0x27);
+    if (result < 0)
+    {
+        return result;
+    }
+    
+    // Read the ZDA bit.
+    return result >> 2 & 1;
+}
+
+int32_t l3gZRead()
+{
+    char reg = 0x80 | 0x2C;  // OUT_Z_L, with slave address updating
+    int result = i2c.write(address, &reg, 1);
+    if (result != 0)
+    {
+        return -500001;
+    }
+    
+    char data[2];
+    result = i2c.read(address, data, 2);
+    if(result != 0)
+    {
+        return -500002;
+    }
+    
+    return (int16_t)(data[1] << 8 | data[0]);
+}
\ No newline at end of file