Diff: BNO055.cpp

Revision:

11:17bc36c5ccbb

Parent:

10:63a9849f0e97

Child:

13:92854c8deb3c

diff -r 63a9849f0e97 -r 17bc36c5ccbb BNO055.cpp
--- a/BNO055.cpp	Sun Jan 28 20:55:10 2018 +0000
+++ b/BNO055.cpp	Sun Jan 28 21:29:12 2018 +0000
@@ -80,14 +80,21 @@
 
 void BNO055::get_quaternion(BNO055_QUATERNION_TypeDef *result)
 {
+    int16_t w,x,y,z;
+
     select_page(0);
     dt[0] = BNO055_QUATERNION_W_LSB;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 8, false);
-    result->w = dt[1] << 8 | dt[0];
-    result->x = dt[3] << 8 | dt[2];
-    result->y = dt[5] << 8 | dt[4];
-    result->z = dt[7] << 8 | dt[6];
+    w = (dt[1] << 8 | dt[0]);
+    x = (dt[3] << 8 | dt[2]);
+    y = (dt[5] << 8 | dt[4]);
+    z = (dt[7] << 8 | dt[6]);
+
+    result->w = double(w) / 16384.0f;
+    result->x = double(x) / 16384.0f;
+    result->y = double(y) / 16384.0f;
+    result->z = double(z) / 16384.0f;
 }
 
 void BNO055::get_linear_accel(BNO055_VECTOR_TypeDef *result)