Diff: BNO055.cpp

Revision:

5:1c085cc12d85

Parent:

4:9e6fead1e93e

Child:

6:662c92c84505

--- a/BNO055.cpp	Thu Apr 16 10:47:40 2015 +0000
+++ b/BNO055.cpp	Mon Apr 10 19:52:37 2017 +0000
@@ -95,6 +95,37 @@
     qua->z = dt[7] << 8 | dt[6];
 }
 
+void BNO055::get_velocities(BNO055_VEL_TypeDef *el)
+{
+    uint8_t deg_or_rad;
+    int16_t x,y,z;
+
+    select_page(0);
+    dt[0] = BNO055_UNIT_SEL;
+    _i2c.write(chip_addr, dt, 1, true);
+    _i2c.read(chip_addr, dt, 1, false);
+    if (dt[0] & 0x02) {
+        deg_or_rad = 1; // Radian per sec
+    } else {
+        deg_or_rad = 0; // Degree per sec
+    }
+    dt[0] = BNO055_GYRO_X_LSB;
+    _i2c.write(chip_addr, dt, 1, true);
+    _i2c.read(chip_addr, dt, 6, false);
+    x = dt[1] << 8 | dt[0];
+    y = dt[3] << 8 | dt[2];
+    z = dt[5] << 8 | dt[4];
+    if (deg_or_rad) {
+        el->x = (double)x / 900;
+        el->y = (double)y / 900;
+        el->z = (double)z / 900;
+    } else {
+        el->x = (double)x / 16;
+        el->y = (double)y / 16;
+        el->z = (double)z / 16;
+    }
+}
+
 void BNO055::get_linear_accel(BNO055_LIN_ACC_TypeDef *la)
 {
     uint8_t ms2_or_mg;
@@ -220,7 +251,7 @@
 {
     select_page(0);
     dt[0] = BNO055_UNIT_SEL;
-    dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C;
+    dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_RPS + UNIT_EULER_RAD + UNIT_TEMP_C;
     _i2c.write(chip_addr, dt, 2, false);
 }