Diff: Gyroscope.cpp

Revision:

11:d21275e60ebb

Parent:

8:cba37530d480

Child:

12:1632d7391453

--- a/Gyroscope.cpp	Tue Feb 24 10:10:14 2015 +0000
+++ b/Gyroscope.cpp	Thu Mar 12 10:16:01 2015 +0000
@@ -56,11 +56,21 @@
 
     write_reg(0x15, 0); // new data interrupt disabled
     
-    const int16_t gx = read_reg(0x02) | (read_reg(0x03) << 8);
-    const int16_t gy = read_reg(0x04) | (read_reg(0x05) << 8);
-    const int16_t gz = read_reg(0x06) | (read_reg(0x07) << 8);
-
+    union {
+        uint8_t bytes[6];
+        struct {
+            int16_t z;
+            int16_t y;
+            int16_t x;
+        } val;
+    } buffer;
+    
+    size_t i = sizeof buffer;
+    while (i --> 0) {
+        buffer.bytes[i] = read_reg(0x02 + i);
+    };
+    
     write_reg(0x15, 1 << 7); // new data interrupt enabled
 
-    return Vector3(gx, gy, gz) * gyro_resolution;
+    return Vector3(buffer.val.x, buffer.val.y, buffer.val.z) * gyro_resolution;
 }