Simon Scott / Mbed 2 deprecated BNO055_Example

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.

Dependencies:   mbed

main.cpp@0:8a8f44689157, 2015-09-17 (annotated)

Committer:
simonscott
Date:
Thu Sep 17 19:31:13 2015 +0000
Revision:
0:8a8f44689157
First version.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simonscott 0:8a8f44689157 1 /**
simonscott 0:8a8f44689157 2 * A simple single-file program to test the BNO055 breakout board from Adafruit.
simonscott 0:8a8f44689157 3 * There are two BNO055 libraries on MBED, as well as Arduino source code on the Adafruit website,
simonscott 0:8a8f44689157 4 * but none of it worked for me, so I wrote this simple example.
simonscott 0:8a8f44689157 5 *
simonscott 0:8a8f44689157 6 * Some notes:
simonscott 0:8a8f44689157 7 * 1.) You will probably need to reset your microcontroller a few times. The BNO055 is correctly initialized when
simonscott 0:8a8f44689157 8 * the ID is read correctly and the temperature is not zero (i.e. initBNO055() should return true).
simonscott 0:8a8f44689157 9 * 2.) The BNO055 breakout board uses an external crystal. If you are not using an external crystal, delete the
simonscott 0:8a8f44689157 10 * I2C write message that sets the chip to using external crystal.
simonscott 0:8a8f44689157 11 */
simonscott 0:8a8f44689157 12
simonscott 0:8a8f44689157 13 #include "mbed.h"
simonscott 0:8a8f44689157 14
simonscott 0:8a8f44689157 15 Serial pc(USBTX,USBRX);
simonscott 0:8a8f44689157 16 I2C i2c(D7, D6);
simonscott 0:8a8f44689157 17
simonscott 0:8a8f44689157 18 const int bno055_addr = 0x28 << 1;
simonscott 0:8a8f44689157 19
simonscott 0:8a8f44689157 20 const int BNO055_ID_ADDR = 0x00;
simonscott 0:8a8f44689157 21 const int BNO055_EULER_H_LSB_ADDR = 0x1A;
simonscott 0:8a8f44689157 22 const int BNO055_TEMP_ADDR = 0x34;
simonscott 0:8a8f44689157 23 const int BNO055_OPR_MODE_ADDR = 0x3D;
simonscott 0:8a8f44689157 24 const int BNO055_CALIB_STAT_ADDR = 0x35;
simonscott 0:8a8f44689157 25 const int BNO055_SYS_STAT_ADDR = 0x39;
simonscott 0:8a8f44689157 26 const int BNO055_SYS_ERR_ADDR = 0x3A;
simonscott 0:8a8f44689157 27 const int BNO055_AXIS_MAP_CONFIG_ADDR = 0x41;
simonscott 0:8a8f44689157 28 const int BNO055_SYS_TRIGGER_ADDR = 0x3F;
simonscott 0:8a8f44689157 29
simonscott 0:8a8f44689157 30 typedef struct CalibStatus_t
simonscott 0:8a8f44689157 31 {
simonscott 0:8a8f44689157 32 int mag;
simonscott 0:8a8f44689157 33 int acc;
simonscott 0:8a8f44689157 34 int gyr;
simonscott 0:8a8f44689157 35 int sys;
simonscott 0:8a8f44689157 36 } CalibStatus;
simonscott 0:8a8f44689157 37
simonscott 0:8a8f44689157 38 typedef struct Euler_t
simonscott 0:8a8f44689157 39 {
simonscott 0:8a8f44689157 40 float heading;
simonscott 0:8a8f44689157 41 float pitch;
simonscott 0:8a8f44689157 42 float roll;
simonscott 0:8a8f44689157 43 } Euler;
simonscott 0:8a8f44689157 44
simonscott 0:8a8f44689157 45
simonscott 0:8a8f44689157 46 /**
simonscott 0:8a8f44689157 47 * Function to write to a single 8-bit register
simonscott 0:8a8f44689157 48 */
simonscott 0:8a8f44689157 49 void writeReg(int regAddr, char value)
simonscott 0:8a8f44689157 50 {
simonscott 0:8a8f44689157 51 char wbuf[2];
simonscott 0:8a8f44689157 52 wbuf[0] = regAddr;
simonscott 0:8a8f44689157 53 wbuf[1] = value;
simonscott 0:8a8f44689157 54 i2c.write(bno055_addr, wbuf, 2, false);
simonscott 0:8a8f44689157 55 }
simonscott 0:8a8f44689157 56
simonscott 0:8a8f44689157 57 /**
simonscott 0:8a8f44689157 58 * Function to read from a single 8-bit register
simonscott 0:8a8f44689157 59 */
simonscott 0:8a8f44689157 60 char readReg(int regAddr)
simonscott 0:8a8f44689157 61 {
simonscott 0:8a8f44689157 62 char rwbuf = regAddr;
simonscott 0:8a8f44689157 63 i2c.write(bno055_addr, &rwbuf, 1, false);
simonscott 0:8a8f44689157 64 i2c.read(bno055_addr, &rwbuf, 1, false);
simonscott 0:8a8f44689157 65 return rwbuf;
simonscott 0:8a8f44689157 66 }
simonscott 0:8a8f44689157 67
simonscott 0:8a8f44689157 68 /**
simonscott 0:8a8f44689157 69 * Returns the calibration status of each component
simonscott 0:8a8f44689157 70 */
simonscott 0:8a8f44689157 71 CalibStatus readCalibrationStatus()
simonscott 0:8a8f44689157 72 {
simonscott 0:8a8f44689157 73 CalibStatus status;
simonscott 0:8a8f44689157 74 int regVal = readReg(BNO055_CALIB_STAT_ADDR);
simonscott 0:8a8f44689157 75
simonscott 0:8a8f44689157 76 status.mag = regVal & 0x03;
simonscott 0:8a8f44689157 77 status.acc = (regVal >> 2) & 0x03;
simonscott 0:8a8f44689157 78 status.gyr = (regVal >> 4) & 0x03;
simonscott 0:8a8f44689157 79 status.sys = (regVal >> 6) & 0x03;
simonscott 0:8a8f44689157 80
simonscott 0:8a8f44689157 81 return status;
simonscott 0:8a8f44689157 82 }
simonscott 0:8a8f44689157 83
simonscott 0:8a8f44689157 84
simonscott 0:8a8f44689157 85 /**
simonscott 0:8a8f44689157 86 * Checks that there are no errors on the accelerometer
simonscott 0:8a8f44689157 87 */
simonscott 0:8a8f44689157 88 bool bno055Healthy()
simonscott 0:8a8f44689157 89 {
simonscott 0:8a8f44689157 90 int sys_error = readReg(BNO055_SYS_ERR_ADDR);
simonscott 0:8a8f44689157 91 wait(0.001);
simonscott 0:8a8f44689157 92 int sys_stat = readReg(BNO055_SYS_STAT_ADDR);
simonscott 0:8a8f44689157 93 wait(0.001);
simonscott 0:8a8f44689157 94
simonscott 0:8a8f44689157 95 if(sys_error == 0 && sys_stat == 5)
simonscott 0:8a8f44689157 96 return true;
simonscott 0:8a8f44689157 97 else
simonscott 0:8a8f44689157 98 return false;
simonscott 0:8a8f44689157 99 }
simonscott 0:8a8f44689157 100
simonscott 0:8a8f44689157 101
simonscott 0:8a8f44689157 102 /**
simonscott 0:8a8f44689157 103 * Configure and initialize the BNO055
simonscott 0:8a8f44689157 104 */
simonscott 0:8a8f44689157 105 bool initBNO055()
simonscott 0:8a8f44689157 106 {
simonscott 0:8a8f44689157 107 unsigned char regVal;
simonscott 0:8a8f44689157 108 i2c.frequency(400000);
simonscott 0:8a8f44689157 109 bool startupPass = true;
simonscott 0:8a8f44689157 110
simonscott 0:8a8f44689157 111 // Do some basic power-up tests
simonscott 0:8a8f44689157 112 regVal = readReg(BNO055_ID_ADDR);
simonscott 0:8a8f44689157 113 if(regVal == 0xA0)
simonscott 0:8a8f44689157 114 pc.printf("BNO055 successfully detected!\r\n");
simonscott 0:8a8f44689157 115 else {
simonscott 0:8a8f44689157 116 pc.printf("ERROR: no BNO055 detected\r\n");
simonscott 0:8a8f44689157 117 startupPass = false;
simonscott 0:8a8f44689157 118 }
simonscott 0:8a8f44689157 119
simonscott 0:8a8f44689157 120 regVal = readReg(BNO055_TEMP_ADDR);
simonscott 0:8a8f44689157 121 pc.printf("Chip temperature is: %d C\r\n", regVal);
simonscott 0:8a8f44689157 122
simonscott 0:8a8f44689157 123 if(regVal == 0)
simonscott 0:8a8f44689157 124 startupPass = false;
simonscott 0:8a8f44689157 125
simonscott 0:8a8f44689157 126 // Change mode to CONFIG
simonscott 0:8a8f44689157 127 writeReg(BNO055_OPR_MODE_ADDR, 0x00);
simonscott 0:8a8f44689157 128 wait(0.2);
simonscott 0:8a8f44689157 129
simonscott 0:8a8f44689157 130 regVal = readReg(BNO055_OPR_MODE_ADDR);
simonscott 0:8a8f44689157 131 pc.printf("Change to mode: %d\r\n", regVal);
simonscott 0:8a8f44689157 132 wait(0.1);
simonscott 0:8a8f44689157 133
simonscott 0:8a8f44689157 134 // Remap axes
simonscott 0:8a8f44689157 135 writeReg(BNO055_AXIS_MAP_CONFIG_ADDR, 0x06); // b00_00_01_10
simonscott 0:8a8f44689157 136 wait(0.1);
simonscott 0:8a8f44689157 137
simonscott 0:8a8f44689157 138 // Set to external crystal
simonscott 0:8a8f44689157 139 writeReg(BNO055_SYS_TRIGGER_ADDR, 0x80);
simonscott 0:8a8f44689157 140 wait(0.2);
simonscott 0:8a8f44689157 141
simonscott 0:8a8f44689157 142 // Change mode to NDOF
simonscott 0:8a8f44689157 143 writeReg(BNO055_OPR_MODE_ADDR, 0x0C);
simonscott 0:8a8f44689157 144 wait(0.2);
simonscott 0:8a8f44689157 145
simonscott 0:8a8f44689157 146 regVal = readReg(BNO055_OPR_MODE_ADDR);
simonscott 0:8a8f44689157 147 pc.printf("Change to mode: %d\r\n", regVal);
simonscott 0:8a8f44689157 148 wait(0.1);
simonscott 0:8a8f44689157 149
simonscott 0:8a8f44689157 150 return startupPass;
simonscott 0:8a8f44689157 151 }
simonscott 0:8a8f44689157 152
simonscott 0:8a8f44689157 153 /**
simonscott 0:8a8f44689157 154 * Reads the Euler angles, zeroed out
simonscott 0:8a8f44689157 155 */
simonscott 0:8a8f44689157 156 Euler getEulerAngles()
simonscott 0:8a8f44689157 157 {
simonscott 0:8a8f44689157 158 char buf[16];
simonscott 0:8a8f44689157 159 Euler e;
simonscott 0:8a8f44689157 160
simonscott 0:8a8f44689157 161 // Read in the Euler angles
simonscott 0:8a8f44689157 162 buf[0] = BNO055_EULER_H_LSB_ADDR;
simonscott 0:8a8f44689157 163 i2c.write(bno055_addr, buf, 1, false);
simonscott 0:8a8f44689157 164 i2c.read(bno055_addr, buf, 6, false);
simonscott 0:8a8f44689157 165
simonscott 0:8a8f44689157 166 short int euler_head = buf[0] + (buf[1] << 8);
simonscott 0:8a8f44689157 167 short int euler_roll = buf[2] + (buf[3] << 8);
simonscott 0:8a8f44689157 168 short int euler_pitch = buf[4] + (buf[5] << 8);
simonscott 0:8a8f44689157 169
simonscott 0:8a8f44689157 170 e.heading = ((float)euler_head) / 16.0;
simonscott 0:8a8f44689157 171 e.roll = ((float)euler_roll) / 16.0;
simonscott 0:8a8f44689157 172 e.pitch = ((float)euler_pitch) / 16.0;
simonscott 0:8a8f44689157 173
simonscott 0:8a8f44689157 174 return e;
simonscott 0:8a8f44689157 175 }
simonscott 0:8a8f44689157 176
simonscott 0:8a8f44689157 177 int main() {
simonscott 0:8a8f44689157 178
simonscott 0:8a8f44689157 179 bool startupPassed;
simonscott 0:8a8f44689157 180 Euler e;
simonscott 0:8a8f44689157 181
simonscott 0:8a8f44689157 182 // Initialize
simonscott 0:8a8f44689157 183 pc.baud(115200);
simonscott 0:8a8f44689157 184 wait(0.5);
simonscott 0:8a8f44689157 185 startupPassed = initBNO055(); // Note: set LED to RED if this fails
simonscott 0:8a8f44689157 186
simonscott 0:8a8f44689157 187 // Read orientation values
simonscott 0:8a8f44689157 188 while(true)
simonscott 0:8a8f44689157 189 {
simonscott 0:8a8f44689157 190 // Make sure that there are no errors
simonscott 0:8a8f44689157 191 if(!bno055Healthy())
simonscott 0:8a8f44689157 192 pc.printf("ERROR: BNO055 has an error/status problem!!!\r\n");
simonscott 0:8a8f44689157 193
simonscott 0:8a8f44689157 194 // Read in the Euler angles
simonscott 0:8a8f44689157 195 e = getEulerAngles();
simonscott 0:8a8f44689157 196
simonscott 0:8a8f44689157 197 wait(0.001);
simonscott 0:8a8f44689157 198
simonscott 0:8a8f44689157 199 // Read in the calibration status
simonscott 0:8a8f44689157 200 CalibStatus calStat = readCalibrationStatus();
simonscott 0:8a8f44689157 201
simonscott 0:8a8f44689157 202 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,
simonscott 0:8a8f44689157 203 calStat.mag, calStat.acc, calStat.gyr, calStat.sys);
simonscott 0:8a8f44689157 204
simonscott 0:8a8f44689157 205 wait(0.05);
simonscott 0:8a8f44689157 206 }
simonscott 0:8a8f44689157 207 }