Revision 2:91dd53e3f7d6, committed 2015-09-17

Comitter:

simonscott

Date:

Thu Sep 17 03:38:32 2015 +0000

Parent:

1:d5386a91bb56

Commit message:

Code now reads acc data.

Changed in this revision

--- a/main.cpp	Wed Sep 16 22:44:23 2015 +0000
+++ b/main.cpp	Thu Sep 17 03:38:32 2015 +0000
@@ -4,7 +4,12 @@
 I2C i2c(D7, D6);
 
 const int addr = 0x28 << 1;
-const int ID_REG = 0x00;
+
+const int BNO055_ID_ADDR                                          = 0x00;
+const int BNO055_EULER_H_LSB_ADDR                                 = 0x1A;
+const int BNO055_OPR_MODE_ADDR                                    = 0x3D;
+const int BNO055_CALIB_STAT_ADDR                                  = 0x35;
+const int BNO055_AXIS_MAP_CONFIG_ADDR                             = 0x41;
 
 int main() {
 
@@ -13,13 +18,80 @@
 
     char buf[16];
     
-    for(int i=0; i < 32; i++)
+    for(int i=0; i < 10; i++)
     {
         buf[0] = i;
         i2c.write(addr, buf, 1, false);
         i2c.read(addr, buf, 1, false);
  
         printf("Data at reg %d: %d\r\n", i, buf[0]);
+        wait(0.1);
+    }
+    pc.printf("\r\n");
+    
+    // Change mode to CONFIG
+    buf[0] = BNO055_OPR_MODE_ADDR;
+    buf[1] = 0x00;
+    i2c.write(addr, buf, 2, false);
+    wait(0.2);
+    
+    buf[0] = BNO055_OPR_MODE_ADDR;
+    i2c.write(addr, buf, 1, false);
+    i2c.read(addr, buf, 1, false);
+    pc.printf("Old mode register: %d\r\n", buf[0]);
+     
+    wait(0.1);
+    
+    // Remap axes
+    buf[0] = BNO055_AXIS_MAP_CONFIG_ADDR;
+    buf[1] = 0x18;  // b00_01_10_00
+    i2c.write(addr, buf, 2, false);
+    wait(0.2);    
+
+    // Change mode to NDOF
+    buf[0] = BNO055_OPR_MODE_ADDR;
+    buf[1] = 0x0C;
+    i2c.write(addr, buf, 2, false);
+    wait(0.2);
+ 
+    buf[0] = BNO055_OPR_MODE_ADDR;
+    i2c.write(addr, buf, 1, false);
+    i2c.read(addr, buf, 1, false);
+    pc.printf("New mode register: %d\r\n", buf[0]);
+
+    wait(0.2);
+    
+    // Read orientation values
+    while(true)
+    {
+        buf[0] = BNO055_EULER_H_LSB_ADDR;
+        i2c.write(addr, buf, 1, false);
+        i2c.read(addr, buf, 6, false);
+        
+        short int euler_head = buf[0] + (buf[1] << 8);
+        short int euler_roll = buf[2] + (buf[3] << 8);
+        short int euler_pitch = buf[4] + (buf[5] << 8);
+ 
+        float euler_head_f = euler_head / 16.0;
+        float euler_roll_f = euler_roll / 16.0;
+        float euler_pitch_f = euler_pitch / 16.0;
+ 
+        wait(0.001);
+ 
+        buf[0] = BNO055_CALIB_STAT_ADDR;
+        i2c.write(addr, buf, 1, false);
+        i2c.read(addr, buf, 1, false);
+        
+        int mag_calib_status = buf[0] & 0x03;
+        int acc_calib_status = (buf[0] >> 2) & 0x03;
+        int gyr_calib_status = (buf[0] >> 4) & 0x03;
+        int sys_calib_status = (buf[0] >> 6) & 0x03;
+
+        printf("Heading: %f \tRoll: %f \tPitch: %f \tMAG: %d ACC: %d GYR: %d SYS: %d\r\n", euler_head_f, euler_roll_f, euler_pitch_f,
+            mag_calib_status, acc_calib_status, gyr_calib_status, sys_calib_status);
+        
         wait(0.5);
     }
+    
+
 }