Adithya Murali / Mbed 2 deprecated hw3

Testing BNO055

Dependencies:   PinDetect USBDevice mbed

main.cpp@0:cfd7fbe195ca, 2015-09-20 (annotated)

Committer:
adithya_murali
Date:
Sun Sep 20 21:23:31 2015 +0000
Revision:
0:cfd7fbe195ca
Testing BNO055

Who changed what in which revision?

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