Diff: BNO055.cpp

Revision:

7:8a893ab512f4

Parent:

6:1f722ffec323

--- a/BNO055.cpp	Tue Sep 22 19:09:45 2015 +0000
+++ b/BNO055.cpp	Thu Jul 21 11:38:53 2016 +0000
@@ -85,7 +85,7 @@
         }
     else {
         rx = rx | units;
-        rate_scale = 1.0f/900.0f;
+        angle_scale = 1.0f/900.0f;
         }
     writechar(BNO055_UNIT_SEL_ADDR,rx);
 }    
@@ -103,11 +103,45 @@
     writechar(BNO055_UNIT_SEL_ADDR,rx);
 }    
 
-void BNO055::set_orientation(char units){
-    readchar(BNO055_UNIT_SEL_ADDR);
-    if (units == WINDOWS) rx = rx &0x7F;
-    else rx = rx | units;
-    writechar(BNO055_UNIT_SEL_ADDR,rx);
+void BNO055::set_orientation(int position){
+    switch(position){
+        case 0:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
+            break;
+        case 1: //(Default)
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+            break;
+        case 2:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x06);
+            break;  
+        case 3:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);            
+            break;
+        case 4:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);            
+            break;
+        case 5:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);            
+            break;
+        case 6:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);            
+            break;
+        case 7:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);            
+            break;
+        default:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+            break;                 
+    }
 }        
 
 void BNO055::setmode(char omode){