Alexander Lill / BNO055_fusion

Extended and refactored library for BNO055, an intelligent 9-axis absolute orientation sensor by Bosch Sensortec. It includes ACC, MAG and GYRO sensors and Cortex-M0 processor.

Fork of BNO055_fusion by Kenji Arai

Please note: pitch and roll in get_euler_angles are switched, the code should be like this:

h = dt[1] << 8 | dt[0]; r = dt[3] << 8 | dt[2]; p = dt[5] << 8 | dt[4];

See https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf

Diff: BNO055.cpp

Revision:
8:46134e7474e7
Parent:
7:f0eb18503c27
Child:
9:e7c8d34bf79a
--- 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;