main.cpp

00001 #include "mbed.h"
00002 
00003 Serial pc(USBTX,USBRX);
00004 I2C i2c(D7, D6);
00005 
00006 const int addr = 0x28 << 1;
00007 
00008 const int BNO055_ID_ADDR                                          = 0x00;
00009 const int BNO055_EULER_H_LSB_ADDR                                 = 0x1A;
00010 const int BNO055_OPR_MODE_ADDR                                    = 0x3D;
00011 const int BNO055_CALIB_STAT_ADDR                                  = 0x35;
00012 const int BNO055_AXIS_MAP_CONFIG_ADDR                             = 0x41;
00013 
00014 int main() {
00015 
00016     // Initialize
00017     i2c.frequency(400000);
00018 
00019     char buf[16];
00020     
00021     for(int i=0; i < 10; i++)
00022     {
00023         buf[0] = i;
00024         i2c.write(addr, buf, 1, false);
00025         i2c.read(addr, buf, 1, false);
00026  
00027         printf("Data at reg %d: %d\r\n", i, buf[0]);
00028         wait(0.1);
00029     }
00030     pc.printf("\r\n");
00031     
00032     // Change mode to CONFIG
00033     buf[0] = BNO055_OPR_MODE_ADDR;
00034     buf[1] = 0x00;
00035     i2c.write(addr, buf, 2, false);
00036     wait(0.2);
00037     
00038     buf[0] = BNO055_OPR_MODE_ADDR;
00039     i2c.write(addr, buf, 1, false);
00040     i2c.read(addr, buf, 1, false);
00041     pc.printf("Old mode register: %d\r\n", buf[0]);
00042      
00043     wait(0.1);
00044     
00045     // Remap axes
00046     buf[0] = BNO055_AXIS_MAP_CONFIG_ADDR;
00047     buf[1] = 0x18;  // b00_01_10_00
00048     i2c.write(addr, buf, 2, false);
00049     wait(0.2);    
00050 
00051     // Change mode to NDOF
00052     buf[0] = BNO055_OPR_MODE_ADDR;
00053     buf[1] = 0x0C;
00054     i2c.write(addr, buf, 2, false);
00055     wait(0.2);
00056  
00057     buf[0] = BNO055_OPR_MODE_ADDR;
00058     i2c.write(addr, buf, 1, false);
00059     i2c.read(addr, buf, 1, false);
00060     pc.printf("New mode register: %d\r\n", buf[0]);
00061 
00062     wait(0.2);
00063     
00064     // Read orientation values
00065     while(true)
00066     {
00067         buf[0] = BNO055_EULER_H_LSB_ADDR;
00068         i2c.write(addr, buf, 1, false);
00069         i2c.read(addr, buf, 6, false);
00070         
00071         short int euler_head = buf[0] + (buf[1] << 8);
00072         short int euler_roll = buf[2] + (buf[3] << 8);
00073         short int euler_pitch = buf[4] + (buf[5] << 8);
00074  
00075         float euler_head_f = euler_head / 16.0;
00076         float euler_roll_f = euler_roll / 16.0;
00077         float euler_pitch_f = euler_pitch / 16.0;
00078  
00079         wait(0.001);
00080  
00081         buf[0] = BNO055_CALIB_STAT_ADDR;
00082         i2c.write(addr, buf, 1, false);
00083         i2c.read(addr, buf, 1, false);
00084         
00085         int mag_calib_status = buf[0] & 0x03;
00086         int acc_calib_status = (buf[0] >> 2) & 0x03;
00087         int gyr_calib_status = (buf[0] >> 4) & 0x03;
00088         int sys_calib_status = (buf[0] >> 6) & 0x03;
00089 
00090         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,
00091             mag_calib_status, acc_calib_status, gyr_calib_status, sys_calib_status);
00092         
00093         wait(0.5);
00094     }
00095     
00096 
00097 }