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?
User | Revision | Line number | New 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 | } |