Dan Perrett / BNO055_fusion

Fork of Alexander Lill's BNO055_fusion library

Diff: BNO055.cpp

Revision:
10:63a9849f0e97
Parent:
9:e7c8d34bf79a
Child:
11:17bc36c5ccbb
--- a/BNO055.cpp	Sun Jan 28 19:46:06 2018 +0000
+++ b/BNO055.cpp	Sun Jan 28 20:55:10 2018 +0000
@@ -57,32 +57,24 @@
 /////////////// Read data & normalize /////////////////////
 void BNO055::get_euler_angles(BNO055_EULER_TypeDef *result)
 {
-    uint8_t deg_or_rad;
     int16_t h,p,r;
 
     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] & 0x04) {
-        deg_or_rad = 1; // Radian
-    } else {
-        deg_or_rad = 0; // Degree
-    }
     dt[0] = BNO055_EULER_H_LSB;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 6, false);
     h = dt[1] << 8 | dt[0];
     p = dt[3] << 8 | dt[2];
     r = dt[5] << 8 | dt[4];
-    if (deg_or_rad) {
+
+    if (use_degrees()) {
+        result->h = (double)h / 16;
+        result->p = (double)p / 16;
+        result->r = (double)r / 16;
+    } else {
         result->h = (double)h / 900;
         result->p = (double)p / 900;
         result->r = (double)r / 900;
-    } else {
-        result->h = (double)h / 16;
-        result->p = (double)p / 16;
-        result->r = (double)r / 16;
     }
 }
 
@@ -100,172 +92,118 @@
 
 void BNO055::get_linear_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_LINEAR_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) {
+
+    if (use_mss()) {
+        result->x = (double)x / 100;
+        result->y = (double)y / 100;
+        result->z = (double)z / 100;
+    } else {
         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_gravity(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_GRAVITY_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) {
+
+    if (use_mss()) {
+        result->x = (double)x / 100;
+        result->y = (double)y / 100;
+        result->z = (double)z / 100;
+    } else {
         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_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;
-    }
+
+    result->x = (double)x;
+    result->y = (double)y;
+    result->z = (double)z;
 }
 
 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) {
+
+    if (use_mss()) {
+        result->x = (double)x / 100;
+        result->y = (double)y / 100;
+        result->z = (double)z / 100;
+    } else {
         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;
+    if (use_dps()) {
+        result->x = (double)x / 16;
+        result->y = (double)y / 16;
+        result->z = (double)z / 16;
     } else {
-        result->x = (double)x / 100;
-        result->y = (double)y / 100;
-        result->z = (double)z / 100;
+        result->x = (double)x / 900;
+        result->y = (double)y / 900;
+        result->z = (double)z / 900;
     }
 }
 
 void BNO055::get_chip_temperature(BNO055_TEMPERATURE_TypeDef *result)
 {
-    uint8_t c_or_f;
+    select_page(0);
 
-    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] & 0x10) {
-        c_or_f = 1; // Fahrenheit
-    } else {
-        c_or_f = 0; // degrees Celsius
-    }
+    uint8_t use_celsius_result = use_celsius();
+
     dt[0] = BNO055_TEMP_SOURCE;
     dt[1] = 0;
     _i2c.write(chip_addr, dt, 2, false);
@@ -273,11 +211,13 @@
     dt[0] = BNO055_TEMP;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 1, false);
-    if (c_or_f) {
-        result->acc_chip = (int8_t)dt[0] * 2;
+
+    if (use_celsius_result) {
+        result->acc_chip = (int8_t)dt[0];
     } else {
-        result->acc_chip = (int8_t)dt[0];
+        result->acc_chip = (int8_t)dt[0] * 2;
     }
+
     dt[0] = BNO055_TEMP_SOURCE;
     dt[1] = 1;
     _i2c.write(chip_addr, dt, 2, false);
@@ -285,10 +225,11 @@
     dt[0] = BNO055_TEMP;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 1, false);
-    if (c_or_f) {
+
+    if (use_celsius_result) {
+        result->gyr_chip = (int8_t)dt[0];
+    } else {
         result->gyr_chip = (int8_t)dt[0] * 2;
-    } else {
-        result->gyr_chip = (int8_t)dt[0];
     }
 }
 
@@ -320,6 +261,50 @@
     _i2c.write(chip_addr, dt, 2, false);
 }
 
+bool BNO055::use_degrees()
+{
+    if (unit_flag_is_set(UNIT_EULER_RAD)) {
+        return false;
+    }
+    return true;
+}
+
+bool BNO055::use_mss()
+{
+    if (unit_flag_is_set(UNIT_ACC_MG)) {
+        return false;
+    }
+    return true;
+}
+
+bool BNO055::use_dps()
+{
+    if (unit_flag_is_set(UNIT_GYR_RPS)) {
+        return false;
+    }
+    return true;
+}
+
+bool BNO055::use_celsius()
+{
+    if (unit_flag_is_set(UNIT_TEMP_F)) {
+        return false;
+    }
+    return true;
+}
+
+bool BNO055::unit_flag_is_set(uint8_t flag)
+{
+    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] & flag) {
+        return true;
+    }
+    return false;
+}
+
 uint8_t BNO055::select_page(uint8_t page)
 {
     if (page != page_flag){