Diff: BNO055.cpp

Revision:

8:46134e7474e7

Parent:

7:f0eb18503c27

Child:

9:e7c8d34bf79a

diff -r f0eb18503c27 -r 46134e7474e7 BNO055.cpp
--- a/BNO055.cpp	Sun Jan 28 19:35:11 2018 +0000
+++ b/BNO055.cpp	Sun Jan 28 19:38:51 2018 +0000
@@ -160,6 +160,99 @@
     }
 }
 
+void BNO055::get_mag(BNO055_VECTOR_TypeDef *result)
+{
+    uint8_t ms2_or_mg;
+    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] & 0x01) {
+        ms2_or_mg = 1; // mg
+    } else {
+        ms2_or_mg = 0; // m/s*s
+    }
+    dt[0] = BNO055_MAG_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 (ms2_or_mg) {
+        result->x = (double)x;
+        result->y = (double)y;
+        result->z = (double)z;
+    } else {
+        result->x = (double)x / 100;
+        result->y = (double)y / 100;
+        result->z = (double)z / 100;
+    }
+}
+
+void BNO055::get_accel(BNO055_VECTOR_TypeDef *result)
+{
+    uint8_t ms2_or_mg;
+    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] & 0x01) {
+        ms2_or_mg = 1; // mg
+    } else {
+        ms2_or_mg = 0; // m/s*s
+    }
+    dt[0] = BNO055_ACC_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 (ms2_or_mg) {
+        result->x = (double)x;
+        result->y = (double)y;
+        result->z = (double)z;
+    } else {
+        result->x = (double)x / 100;
+        result->y = (double)y / 100;
+        result->z = (double)z / 100;
+    }
+}
+
+void BNO055::get_gyro(BNO055_VECTOR_TypeDef *result)
+{
+    uint8_t ms2_or_mg;
+    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] & 0x01) {
+        ms2_or_mg = 1; // mg
+    } else {
+        ms2_or_mg = 0; // m/s*s
+    }
+    dt[0] = BNO055_GYR_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 (ms2_or_mg) {
+        result->x = (double)x;
+        result->y = (double)y;
+        result->z = (double)z;
+    } else {
+        result->x = (double)x / 100;
+        result->y = (double)y / 100;
+        result->z = (double)z / 100;
+    }
+}
+
 void BNO055::get_chip_temperature(BNO055_TEMPERATURE_TypeDef *result)
 {
     uint8_t c_or_f;