Diff: L3GD20.cpp

Revision:

0:a1d1374eea97

Child:

1:191e963655c2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20.cpp	Mon Dec 17 12:47:04 2018 +0000
@@ -0,0 +1,41 @@
+#include "L3GD20.h"
+
+L3GD20::L3GD20(PinName sda, PinName scl) : i2c(sda, scl), address(0xD6)
+{
+    writeByte(0x20, 0x0F);
+}
+
+L3GD20::~L3GD20()
+{
+}
+
+void L3GD20::readData(double &x, double &y, double &z)
+{
+    static const double rate = 0.00875;
+    char h, l;
+    l = readByte(0x28);
+    h = readByte(0x29);
+    x = (short)(h << 8 | l) * rate;
+    l = readByte(0x2A);
+    h = readByte(0x2B);
+    y = (short)(h << 8 | l) * rate;
+    l = readByte(0x2C);
+    h = readByte(0x2D);
+    z = (short)(h << 8 | l) * rate;
+}
+
+char L3GD20::readByte(char reg)
+{
+    i2c.write(address, &reg, 1);
+    char data;
+    i2c.read(address, &data, 1);
+    return data;
+}
+
+void L3GD20::writeByte(char reg, char data)
+{
+    char buf[2];
+    buf[0] = reg;
+    buf[1] = data;
+    i2c.write(address, buf, 2);
+}