![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
A simple single-file program to test the BNO055 breakout board from Adafruit. There are two BNO055 libraries on MBED, as well as Arduino source code on the Adafruit website, but none of it worked for me, so I wrote this simple example.
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 }
Generated on Tue Jul 12 2022 21:33:47 by
![doxygen](doxygen.png)