main.cpp

00001 /**
00002  * A simple single-file program to test the BNO055 breakout board from Adafruit.
00003  * There are two BNO055 libraries on MBED, as well as Arduino source code on the Adafruit website,
00004  * but none of it worked for me, so I wrote this simple example.
00005  *
00006  * Some notes:
00007  * 1.) You will probably need to reset your microcontroller a few times. The BNO055 is correctly initialized when
00008  *      the ID is read correctly and the temperature is not zero (i.e. initBNO055() should return true).
00009  * 2.) The BNO055 breakout board uses an external crystal. If you are not using an external crystal, delete the
00010  *      I2C write message that sets the chip to using external crystal.
00011  */
00012 
00013 #include "mbed.h"
00014 
00015 Serial pc(USBTX,USBRX);
00016 I2C i2c(D7, D6);
00017 
00018 const int bno055_addr = 0x28 << 1;
00019 
00020 const int BNO055_ID_ADDR                                          = 0x00;
00021 const int BNO055_EULER_H_LSB_ADDR                                 = 0x1A;
00022 const int BNO055_TEMP_ADDR                                        = 0x34;
00023 const int BNO055_OPR_MODE_ADDR                                    = 0x3D;
00024 const int BNO055_CALIB_STAT_ADDR                                  = 0x35;
00025 const int BNO055_SYS_STAT_ADDR                                    = 0x39;
00026 const int BNO055_SYS_ERR_ADDR                                     = 0x3A;
00027 const int BNO055_AXIS_MAP_CONFIG_ADDR                             = 0x41;
00028 const int BNO055_SYS_TRIGGER_ADDR                                 = 0x3F;
00029 
00030 typedef struct CalibStatus_t
00031 {
00032     int mag;
00033     int acc;
00034     int gyr;
00035     int sys;
00036 } CalibStatus;
00037 
00038 typedef struct Euler_t
00039 {
00040     float heading;
00041     float pitch;
00042     float roll;
00043 } Euler;
00044 
00045 
00046 /**
00047  * Function to write to a single 8-bit register
00048  */
00049 void writeReg(int regAddr, char value)
00050 {
00051     char wbuf[2];
00052     wbuf[0] = regAddr;
00053     wbuf[1] = value;
00054     i2c.write(bno055_addr, wbuf, 2, false);  
00055 }
00056 
00057 /**
00058  * Function to read from a single 8-bit register
00059  */
00060 char readReg(int regAddr)
00061 {
00062     char rwbuf = regAddr;
00063     i2c.write(bno055_addr, &rwbuf, 1, false);
00064     i2c.read(bno055_addr, &rwbuf, 1, false);
00065     return rwbuf;
00066 }
00067 
00068 /**
00069  * Returns the calibration status of each component
00070  */
00071 CalibStatus readCalibrationStatus()
00072 {
00073     CalibStatus status;
00074     int regVal = readReg(BNO055_CALIB_STAT_ADDR);
00075         
00076     status.mag = regVal & 0x03;
00077     status.acc = (regVal >> 2) & 0x03;
00078     status.gyr = (regVal >> 4) & 0x03;
00079     status.sys = (regVal >> 6) & 0x03;
00080     
00081     return status;
00082 }
00083 
00084 
00085 /**
00086  * Checks that there are no errors on the accelerometer
00087  */
00088 bool bno055Healthy()
00089 {
00090     int sys_error = readReg(BNO055_SYS_ERR_ADDR);
00091     wait(0.001);
00092     int sys_stat = readReg(BNO055_SYS_STAT_ADDR);
00093     wait(0.001);
00094     
00095     if(sys_error == 0 && sys_stat == 5)
00096         return true;
00097     else
00098         return false;
00099 }
00100     
00101 
00102 /**
00103  * Configure and initialize the BNO055
00104  */
00105 bool initBNO055()
00106 {
00107     unsigned char regVal;
00108     i2c.frequency(400000);
00109     bool startupPass = true;
00110     
00111     // Do some basic power-up tests
00112     regVal = readReg(BNO055_ID_ADDR);
00113     if(regVal == 0xA0)
00114         pc.printf("BNO055 successfully detected!\r\n");
00115     else {
00116         pc.printf("ERROR: no BNO055 detected\r\n");
00117         startupPass = false;
00118     }
00119         
00120     regVal = readReg(BNO055_TEMP_ADDR);
00121     pc.printf("Chip temperature is: %d C\r\n", regVal);
00122     
00123     if(regVal == 0)
00124         startupPass = false;
00125  
00126     // Change mode to CONFIG
00127     writeReg(BNO055_OPR_MODE_ADDR, 0x00);
00128     wait(0.2);
00129     
00130     regVal = readReg(BNO055_OPR_MODE_ADDR);
00131     pc.printf("Change to mode: %d\r\n", regVal);
00132     wait(0.1);
00133     
00134     // Remap axes
00135     writeReg(BNO055_AXIS_MAP_CONFIG_ADDR, 0x06);    // b00_00_01_10
00136     wait(0.1);    
00137 
00138     // Set to external crystal
00139     writeReg(BNO055_SYS_TRIGGER_ADDR, 0x80);
00140     wait(0.2);    
00141 
00142     // Change mode to NDOF
00143     writeReg(BNO055_OPR_MODE_ADDR, 0x0C);
00144     wait(0.2);
00145  
00146     regVal = readReg(BNO055_OPR_MODE_ADDR);
00147     pc.printf("Change to mode: %d\r\n", regVal);
00148     wait(0.1);
00149     
00150     return startupPass;
00151 }
00152 
00153 /**
00154  * Reads the Euler angles, zeroed out
00155  */
00156 Euler getEulerAngles()
00157 {
00158     char buf[16];
00159     Euler e;
00160     
00161     // Read in the Euler angles
00162     buf[0] = BNO055_EULER_H_LSB_ADDR;
00163     i2c.write(bno055_addr, buf, 1, false);
00164     i2c.read(bno055_addr, buf, 6, false);
00165     
00166     short int euler_head = buf[0] + (buf[1] << 8);
00167     short int euler_roll = buf[2] + (buf[3] << 8);
00168     short int euler_pitch = buf[4] + (buf[5] << 8);
00169     
00170     e.heading = ((float)euler_head) / 16.0;
00171     e.roll = ((float)euler_roll) / 16.0;
00172     e.pitch = ((float)euler_pitch) / 16.0;
00173     
00174     return e;
00175 }
00176 
00177 int main() {
00178 
00179     bool startupPassed;
00180     Euler e;
00181 
00182     // Initialize
00183     pc.baud(115200);
00184     wait(0.5);
00185     startupPassed = initBNO055();   // Note: set LED to RED if this fails
00186  
00187     // Read orientation values
00188     while(true)
00189     {
00190         // Make sure that there are no errors
00191         if(!bno055Healthy())
00192             pc.printf("ERROR: BNO055 has an error/status problem!!!\r\n");
00193         
00194         // Read in the Euler angles
00195         e = getEulerAngles();
00196  
00197         wait(0.001);
00198  
00199         // Read in the calibration status
00200         CalibStatus calStat = readCalibrationStatus();
00201         
00202         printf("Heading: %7.2f \tRoll: %7.2f \tPitch: %7.2f \tMAG: %d ACC: %d GYR: %d SYS: %d\r\n", e.heading, e.roll, e.pitch,
00203             calStat.mag, calStat.acc, calStat.gyr, calStat.sys);
00204         
00205         wait(0.05);
00206     }
00207 }