Diff: BNO055_lib.cpp

Revision:

1:6de1be86a23f

Parent:

0:4c82133c291f

--- a/BNO055_lib.cpp	Tue Jan 22 11:47:28 2019 +0000
+++ b/BNO055_lib.cpp	Tue Feb 19 14:57:28 2019 +0000
@@ -25,18 +25,52 @@
 }
 
 void BNO055_lib::setAccRange(ACC_RANGE range){
+    cmd[0] = BNO055_PAGE_ID;
+    cmd[1] = 0x01;
+    i2c -> write(slave << 1, cmd, 2);
+    
     cmd[0] = BNO055_ACC_CONFIG;
-    cmd[1] = 0x0F;
+    cmd[1] = (char)range | 0x0C;
+    i2c -> write(slave << 1, cmd, 2);
+    i2c -> write(slave << 1, cmd, 1);
+    i2c -> read(slave << 1 | 1, buff, 1);
+    
+    cmd[0] = BNO055_PAGE_ID;
+    cmd[1] = 0x00;
     i2c -> write(slave << 1, cmd, 2);
 }
 
 void BNO055_lib::setOperationMode(OPERATION_MODE mode){
+    wait(0.05f);
     cmd[0] = BNO055_OPR_MODE;
-    //cmd[1] = 0x07; //AMG
-    cmd[1] = 0x0C; //NDOF
+    cmd[1] = (char)mode;
+    i2c -> write(slave << 1, cmd, 2);
+    wait(0.05f);
+}
+
+void BNO055_lib::setAxis(AXIS x, AXIS y, AXIS z){
+    if((x != y) && (y != z) && (z != x)){
+        cmd[0] = BNO055_AXIS_MAP_CONFIG;
+        cmd[1] = ((char)z << 4) | ((char)y << 2) | (char)x;
+        i2c -> write(slave << 1, cmd, 2);
+    }
+}
+
+void BNO055_lib::setAxisPM(int x_pm, int y_pm, int z_pm){
+    cmd[0] = BNO055_AXIS_MAP_SIGN;
+    cmd[1] = 0x00;
+    if(x_pm < 0){
+        cmd[1] = cmd[1] | 0x04;
+    }
+    if(y_pm < 0){
+        cmd[1] = cmd[1] | 0x02;
+    }
+    if(z_pm < 0){
+        cmd[1] = cmd[1] | 0x01;
+    }
     i2c -> write(slave << 1, cmd, 2);
 }
-    
+
 void BNO055_lib::getAcc(float *acc){
     cmd[0] = BNO055_ACC_DATA;
     i2c -> write(slave << 1, cmd, 1);