Dan Perrett / BNO055_fusion

Fork of Alexander Lill's BNO055_fusion library

BNO055.cpp@8:46134e7474e7, 2018-01-28 (annotated)

Committer:
AlexanderLill
Date:
Sun Jan 28 19:38:51 2018 +0000
Revision:
8:46134e7474e7
Parent:
7:f0eb18503c27
Child:
9:e7c8d34bf79a
WIP: Add functions for retrieving mag, accel and gyro

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenjiArai 0:86a17116e8be 1 /*
kenjiArai 0:86a17116e8be 2 * mbed library program
kenjiArai 0:86a17116e8be 3 * BNO055 Intelligent 9-axis absolute orientation sensor
kenjiArai 0:86a17116e8be 4 * by Bosch Sensortec
kenjiArai 0:86a17116e8be 5 *
kenjiArai 5:cf33bcfe976c 6 * Copyright (c) 2015,'17 Kenji Arai / JH1PJL
kenjiArai 0:86a17116e8be 7 * http://www.page.sannet.ne.jp/kenjia/index.html
kenjiArai 0:86a17116e8be 8 * http://mbed.org/users/kenjiArai/
kenjiArai 0:86a17116e8be 9 * Created: March 30th, 2015
kenjiArai 6:07d01bf36ad0 10 * Revised: August 23rd, 2017
kenjiArai 0:86a17116e8be 11 */
kenjiArai 0:86a17116e8be 12
kenjiArai 0:86a17116e8be 13 #include "mbed.h"
kenjiArai 0:86a17116e8be 14 #include "BNO055.h"
kenjiArai 0:86a17116e8be 15
kenjiArai 5:cf33bcfe976c 16
kenjiArai 5:cf33bcfe976c 17 #if MBED_MAJOR_VERSION == 2
kenjiArai 5:cf33bcfe976c 18 #define WAIT_MS(x) wait_ms(x)
kenjiArai 5:cf33bcfe976c 19 #elif MBED_MAJOR_VERSION == 5
kenjiArai 5:cf33bcfe976c 20 #define WAIT_MS(x) Thread::wait(x)
kenjiArai 5:cf33bcfe976c 21 #else
kenjiArai 5:cf33bcfe976c 22 #error "Running on Unknown OS"
kenjiArai 5:cf33bcfe976c 23 #endif
kenjiArai 5:cf33bcfe976c 24
kenjiArai 1:cb7e19c0a702 25 BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode):
kenjiArai 5:cf33bcfe976c 26 _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p), _res(p_reset)
kenjiArai 0:86a17116e8be 27 {
kenjiArai 0:86a17116e8be 28 chip_addr = addr;
kenjiArai 0:86a17116e8be 29 chip_mode = mode;
kenjiArai 0:86a17116e8be 30 initialize ();
kenjiArai 0:86a17116e8be 31 }
kenjiArai 0:86a17116e8be 32
kenjiArai 1:cb7e19c0a702 33 BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset) :
kenjiArai 5:cf33bcfe976c 34 _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p), _res(p_reset)
kenjiArai 0:86a17116e8be 35 {
kenjiArai 0:86a17116e8be 36 chip_addr = BNO055_G_CHIP_ADDR;
kenjiArai 0:86a17116e8be 37 chip_mode = MODE_NDOF;
kenjiArai 0:86a17116e8be 38 initialize ();
kenjiArai 0:86a17116e8be 39 }
kenjiArai 0:86a17116e8be 40
kenjiArai 1:cb7e19c0a702 41 BNO055::BNO055 (I2C& p_i2c, PinName p_reset, uint8_t addr, uint8_t mode) :
kenjiArai 1:cb7e19c0a702 42 _i2c(p_i2c), _res(p_reset)
kenjiArai 0:86a17116e8be 43 {
kenjiArai 0:86a17116e8be 44 chip_addr = addr;
kenjiArai 0:86a17116e8be 45 chip_mode = mode;
kenjiArai 0:86a17116e8be 46 initialize ();
kenjiArai 0:86a17116e8be 47 }
kenjiArai 0:86a17116e8be 48
kenjiArai 1:cb7e19c0a702 49 BNO055::BNO055 (I2C& p_i2c, PinName p_reset) :
kenjiArai 1:cb7e19c0a702 50 _i2c(p_i2c), _res(p_reset)
kenjiArai 0:86a17116e8be 51 {
kenjiArai 0:86a17116e8be 52 chip_addr = BNO055_G_CHIP_ADDR;
kenjiArai 0:86a17116e8be 53 chip_mode = MODE_NDOF;
kenjiArai 0:86a17116e8be 54 initialize ();
kenjiArai 0:86a17116e8be 55 }
kenjiArai 0:86a17116e8be 56
kenjiArai 0:86a17116e8be 57 /////////////// Read data & normalize /////////////////////
AlexanderLill 7:f0eb18503c27 58 void BNO055::get_euler_angles(BNO055_EULER_TypeDef *result)
kenjiArai 0:86a17116e8be 59 {
kenjiArai 0:86a17116e8be 60 uint8_t deg_or_rad;
kenjiArai 0:86a17116e8be 61 int16_t h,p,r;
kenjiArai 0:86a17116e8be 62
kenjiArai 0:86a17116e8be 63 select_page(0);
kenjiArai 0:86a17116e8be 64 dt[0] = BNO055_UNIT_SEL;
kenjiArai 0:86a17116e8be 65 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 66 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:86a17116e8be 67 if (dt[0] & 0x04) {
kenjiArai 0:86a17116e8be 68 deg_or_rad = 1; // Radian
kenjiArai 0:86a17116e8be 69 } else {
kenjiArai 0:86a17116e8be 70 deg_or_rad = 0; // Degree
kenjiArai 0:86a17116e8be 71 }
kenjiArai 0:86a17116e8be 72 dt[0] = BNO055_EULER_H_LSB;
kenjiArai 0:86a17116e8be 73 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 74 _i2c.read(chip_addr, dt, 6, false);
kenjiArai 0:86a17116e8be 75 h = dt[1] << 8 | dt[0];
kenjiArai 0:86a17116e8be 76 p = dt[3] << 8 | dt[2];
kenjiArai 0:86a17116e8be 77 r = dt[5] << 8 | dt[4];
kenjiArai 0:86a17116e8be 78 if (deg_or_rad) {
AlexanderLill 7:f0eb18503c27 79 result->h = (double)h / 900;
AlexanderLill 7:f0eb18503c27 80 result->p = (double)p / 900;
AlexanderLill 7:f0eb18503c27 81 result->r = (double)r / 900;
kenjiArai 0:86a17116e8be 82 } else {
AlexanderLill 7:f0eb18503c27 83 result->h = (double)h / 16;
AlexanderLill 7:f0eb18503c27 84 result->p = (double)p / 16;
AlexanderLill 7:f0eb18503c27 85 result->r = (double)r / 16;
kenjiArai 0:86a17116e8be 86 }
kenjiArai 0:86a17116e8be 87 }
kenjiArai 0:86a17116e8be 88
AlexanderLill 7:f0eb18503c27 89 void BNO055::get_quaternion(BNO055_QUATERNION_TypeDef *result)
kenjiArai 0:86a17116e8be 90 {
kenjiArai 0:86a17116e8be 91 select_page(0);
kenjiArai 0:86a17116e8be 92 dt[0] = BNO055_QUATERNION_W_LSB;
kenjiArai 0:86a17116e8be 93 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 94 _i2c.read(chip_addr, dt, 8, false);
AlexanderLill 7:f0eb18503c27 95 result->w = dt[1] << 8 | dt[0];
AlexanderLill 7:f0eb18503c27 96 result->x = dt[3] << 8 | dt[2];
AlexanderLill 7:f0eb18503c27 97 result->y = dt[5] << 8 | dt[4];
AlexanderLill 7:f0eb18503c27 98 result->z = dt[7] << 8 | dt[6];
kenjiArai 0:86a17116e8be 99 }
kenjiArai 0:86a17116e8be 100
AlexanderLill 7:f0eb18503c27 101 void BNO055::get_linear_accel(BNO055_VECTOR_TypeDef *result)
kenjiArai 0:86a17116e8be 102 {
kenjiArai 0:86a17116e8be 103 uint8_t ms2_or_mg;
kenjiArai 0:86a17116e8be 104 int16_t x,y,z;
kenjiArai 0:86a17116e8be 105
kenjiArai 0:86a17116e8be 106 select_page(0);
kenjiArai 0:86a17116e8be 107 dt[0] = BNO055_UNIT_SEL;
kenjiArai 0:86a17116e8be 108 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 109 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:86a17116e8be 110 if (dt[0] & 0x01) {
kenjiArai 0:86a17116e8be 111 ms2_or_mg = 1; // mg
kenjiArai 0:86a17116e8be 112 } else {
kenjiArai 0:86a17116e8be 113 ms2_or_mg = 0; // m/s*s
kenjiArai 0:86a17116e8be 114 }
kenjiArai 0:86a17116e8be 115 dt[0] = BNO055_LINEAR_ACC_X_LSB;
kenjiArai 0:86a17116e8be 116 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 117 _i2c.read(chip_addr, dt, 6, false);
kenjiArai 0:86a17116e8be 118 x = dt[1] << 8 | dt[0];
kenjiArai 0:86a17116e8be 119 y = dt[3] << 8 | dt[2];
kenjiArai 0:86a17116e8be 120 z = dt[5] << 8 | dt[4];
kenjiArai 0:86a17116e8be 121 if (ms2_or_mg) {
AlexanderLill 7:f0eb18503c27 122 result->x = (double)x;
AlexanderLill 7:f0eb18503c27 123 result->y = (double)y;
AlexanderLill 7:f0eb18503c27 124 result->z = (double)z;
kenjiArai 0:86a17116e8be 125 } else {
AlexanderLill 7:f0eb18503c27 126 result->x = (double)x / 100;
AlexanderLill 7:f0eb18503c27 127 result->y = (double)y / 100;
AlexanderLill 7:f0eb18503c27 128 result->z = (double)z / 100;
kenjiArai 0:86a17116e8be 129 }
kenjiArai 0:86a17116e8be 130 }
kenjiArai 0:86a17116e8be 131
AlexanderLill 7:f0eb18503c27 132 void BNO055::get_gravity(BNO055_VECTOR_TypeDef *result)
kenjiArai 0:86a17116e8be 133 {
kenjiArai 0:86a17116e8be 134 uint8_t ms2_or_mg;
kenjiArai 0:86a17116e8be 135 int16_t x,y,z;
kenjiArai 0:86a17116e8be 136
kenjiArai 0:86a17116e8be 137 select_page(0);
kenjiArai 0:86a17116e8be 138 dt[0] = BNO055_UNIT_SEL;
kenjiArai 0:86a17116e8be 139 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 140 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:86a17116e8be 141 if (dt[0] & 0x01) {
kenjiArai 0:86a17116e8be 142 ms2_or_mg = 1; // mg
kenjiArai 0:86a17116e8be 143 } else {
kenjiArai 0:86a17116e8be 144 ms2_or_mg = 0; // m/s*s
kenjiArai 0:86a17116e8be 145 }
kenjiArai 0:86a17116e8be 146 dt[0] = BNO055_GRAVITY_X_LSB;
kenjiArai 0:86a17116e8be 147 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 148 _i2c.read(chip_addr, dt, 6, false);
kenjiArai 0:86a17116e8be 149 x = dt[1] << 8 | dt[0];
kenjiArai 0:86a17116e8be 150 y = dt[3] << 8 | dt[2];
kenjiArai 0:86a17116e8be 151 z = dt[5] << 8 | dt[4];
kenjiArai 0:86a17116e8be 152 if (ms2_or_mg) {
AlexanderLill 7:f0eb18503c27 153 result->x = (double)x;
AlexanderLill 7:f0eb18503c27 154 result->y = (double)y;
AlexanderLill 7:f0eb18503c27 155 result->z = (double)z;
kenjiArai 0:86a17116e8be 156 } else {
AlexanderLill 7:f0eb18503c27 157 result->x = (double)x / 100;
AlexanderLill 7:f0eb18503c27 158 result->y = (double)y / 100;
AlexanderLill 7:f0eb18503c27 159 result->z = (double)z / 100;
kenjiArai 0:86a17116e8be 160 }
kenjiArai 0:86a17116e8be 161 }
kenjiArai 0:86a17116e8be 162
AlexanderLill 8:46134e7474e7 163 void BNO055::get_mag(BNO055_VECTOR_TypeDef *result)
AlexanderLill 8:46134e7474e7 164 {
AlexanderLill 8:46134e7474e7 165 uint8_t ms2_or_mg;
AlexanderLill 8:46134e7474e7 166 int16_t x,y,z;
AlexanderLill 8:46134e7474e7 167
AlexanderLill 8:46134e7474e7 168 select_page(0);
AlexanderLill 8:46134e7474e7 169 dt[0] = BNO055_UNIT_SEL;
AlexanderLill 8:46134e7474e7 170 _i2c.write(chip_addr, dt, 1, true);
AlexanderLill 8:46134e7474e7 171 _i2c.read(chip_addr, dt, 1, false);
AlexanderLill 8:46134e7474e7 172 if (dt[0] & 0x01) {
AlexanderLill 8:46134e7474e7 173 ms2_or_mg = 1; // mg
AlexanderLill 8:46134e7474e7 174 } else {
AlexanderLill 8:46134e7474e7 175 ms2_or_mg = 0; // m/s*s
AlexanderLill 8:46134e7474e7 176 }
AlexanderLill 8:46134e7474e7 177 dt[0] = BNO055_MAG_X_LSB;
AlexanderLill 8:46134e7474e7 178 _i2c.write(chip_addr, dt, 1, true);
AlexanderLill 8:46134e7474e7 179 _i2c.read(chip_addr, dt, 6, false);
AlexanderLill 8:46134e7474e7 180 x = dt[1] << 8 | dt[0];
AlexanderLill 8:46134e7474e7 181 y = dt[3] << 8 | dt[2];
AlexanderLill 8:46134e7474e7 182 z = dt[5] << 8 | dt[4];
AlexanderLill 8:46134e7474e7 183 if (ms2_or_mg) {
AlexanderLill 8:46134e7474e7 184 result->x = (double)x;
AlexanderLill 8:46134e7474e7 185 result->y = (double)y;
AlexanderLill 8:46134e7474e7 186 result->z = (double)z;
AlexanderLill 8:46134e7474e7 187 } else {
AlexanderLill 8:46134e7474e7 188 result->x = (double)x / 100;
AlexanderLill 8:46134e7474e7 189 result->y = (double)y / 100;
AlexanderLill 8:46134e7474e7 190 result->z = (double)z / 100;
AlexanderLill 8:46134e7474e7 191 }
AlexanderLill 8:46134e7474e7 192 }
AlexanderLill 8:46134e7474e7 193
AlexanderLill 8:46134e7474e7 194 void BNO055::get_accel(BNO055_VECTOR_TypeDef *result)
AlexanderLill 8:46134e7474e7 195 {
AlexanderLill 8:46134e7474e7 196 uint8_t ms2_or_mg;
AlexanderLill 8:46134e7474e7 197 int16_t x,y,z;
AlexanderLill 8:46134e7474e7 198
AlexanderLill 8:46134e7474e7 199 select_page(0);
AlexanderLill 8:46134e7474e7 200 dt[0] = BNO055_UNIT_SEL;
AlexanderLill 8:46134e7474e7 201 _i2c.write(chip_addr, dt, 1, true);
AlexanderLill 8:46134e7474e7 202 _i2c.read(chip_addr, dt, 1, false);
AlexanderLill 8:46134e7474e7 203 if (dt[0] & 0x01) {
AlexanderLill 8:46134e7474e7 204 ms2_or_mg = 1; // mg
AlexanderLill 8:46134e7474e7 205 } else {
AlexanderLill 8:46134e7474e7 206 ms2_or_mg = 0; // m/s*s
AlexanderLill 8:46134e7474e7 207 }
AlexanderLill 8:46134e7474e7 208 dt[0] = BNO055_ACC_X_LSB;
AlexanderLill 8:46134e7474e7 209 _i2c.write(chip_addr, dt, 1, true);
AlexanderLill 8:46134e7474e7 210 _i2c.read(chip_addr, dt, 6, false);
AlexanderLill 8:46134e7474e7 211 x = dt[1] << 8 | dt[0];
AlexanderLill 8:46134e7474e7 212 y = dt[3] << 8 | dt[2];
AlexanderLill 8:46134e7474e7 213 z = dt[5] << 8 | dt[4];
AlexanderLill 8:46134e7474e7 214 if (ms2_or_mg) {
AlexanderLill 8:46134e7474e7 215 result->x = (double)x;
AlexanderLill 8:46134e7474e7 216 result->y = (double)y;
AlexanderLill 8:46134e7474e7 217 result->z = (double)z;
AlexanderLill 8:46134e7474e7 218 } else {
AlexanderLill 8:46134e7474e7 219 result->x = (double)x / 100;
AlexanderLill 8:46134e7474e7 220 result->y = (double)y / 100;
AlexanderLill 8:46134e7474e7 221 result->z = (double)z / 100;
AlexanderLill 8:46134e7474e7 222 }
AlexanderLill 8:46134e7474e7 223 }
AlexanderLill 8:46134e7474e7 224
AlexanderLill 8:46134e7474e7 225 void BNO055::get_gyro(BNO055_VECTOR_TypeDef *result)
AlexanderLill 8:46134e7474e7 226 {
AlexanderLill 8:46134e7474e7 227 uint8_t ms2_or_mg;
AlexanderLill 8:46134e7474e7 228 int16_t x,y,z;
AlexanderLill 8:46134e7474e7 229
AlexanderLill 8:46134e7474e7 230 select_page(0);
AlexanderLill 8:46134e7474e7 231 dt[0] = BNO055_UNIT_SEL;
AlexanderLill 8:46134e7474e7 232 _i2c.write(chip_addr, dt, 1, true);
AlexanderLill 8:46134e7474e7 233 _i2c.read(chip_addr, dt, 1, false);
AlexanderLill 8:46134e7474e7 234 if (dt[0] & 0x01) {
AlexanderLill 8:46134e7474e7 235 ms2_or_mg = 1; // mg
AlexanderLill 8:46134e7474e7 236 } else {
AlexanderLill 8:46134e7474e7 237 ms2_or_mg = 0; // m/s*s
AlexanderLill 8:46134e7474e7 238 }
AlexanderLill 8:46134e7474e7 239 dt[0] = BNO055_GYR_X_LSB;
AlexanderLill 8:46134e7474e7 240 _i2c.write(chip_addr, dt, 1, true);
AlexanderLill 8:46134e7474e7 241 _i2c.read(chip_addr, dt, 6, false);
AlexanderLill 8:46134e7474e7 242 x = dt[1] << 8 | dt[0];
AlexanderLill 8:46134e7474e7 243 y = dt[3] << 8 | dt[2];
AlexanderLill 8:46134e7474e7 244 z = dt[5] << 8 | dt[4];
AlexanderLill 8:46134e7474e7 245 if (ms2_or_mg) {
AlexanderLill 8:46134e7474e7 246 result->x = (double)x;
AlexanderLill 8:46134e7474e7 247 result->y = (double)y;
AlexanderLill 8:46134e7474e7 248 result->z = (double)z;
AlexanderLill 8:46134e7474e7 249 } else {
AlexanderLill 8:46134e7474e7 250 result->x = (double)x / 100;
AlexanderLill 8:46134e7474e7 251 result->y = (double)y / 100;
AlexanderLill 8:46134e7474e7 252 result->z = (double)z / 100;
AlexanderLill 8:46134e7474e7 253 }
AlexanderLill 8:46134e7474e7 254 }
AlexanderLill 8:46134e7474e7 255
AlexanderLill 7:f0eb18503c27 256 void BNO055::get_chip_temperature(BNO055_TEMPERATURE_TypeDef *result)
kenjiArai 0:86a17116e8be 257 {
kenjiArai 0:86a17116e8be 258 uint8_t c_or_f;
kenjiArai 0:86a17116e8be 259
kenjiArai 0:86a17116e8be 260 select_page(0);
kenjiArai 0:86a17116e8be 261 dt[0] = BNO055_UNIT_SEL;
kenjiArai 0:86a17116e8be 262 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 263 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:86a17116e8be 264 if (dt[0] & 0x10) {
kenjiArai 0:86a17116e8be 265 c_or_f = 1; // Fahrenheit
kenjiArai 0:86a17116e8be 266 } else {
kenjiArai 0:86a17116e8be 267 c_or_f = 0; // degrees Celsius
kenjiArai 0:86a17116e8be 268 }
kenjiArai 0:86a17116e8be 269 dt[0] = BNO055_TEMP_SOURCE;
kenjiArai 0:86a17116e8be 270 dt[1] = 0;
kenjiArai 0:86a17116e8be 271 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 5:cf33bcfe976c 272 WAIT_MS(1); // Do I need to wait?
kenjiArai 0:86a17116e8be 273 dt[0] = BNO055_TEMP;
kenjiArai 0:86a17116e8be 274 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 275 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:86a17116e8be 276 if (c_or_f) {
AlexanderLill 7:f0eb18503c27 277 result->acc_chip = (int8_t)dt[0] * 2;
kenjiArai 0:86a17116e8be 278 } else {
AlexanderLill 7:f0eb18503c27 279 result->acc_chip = (int8_t)dt[0];
kenjiArai 0:86a17116e8be 280 }
kenjiArai 0:86a17116e8be 281 dt[0] = BNO055_TEMP_SOURCE;
kenjiArai 0:86a17116e8be 282 dt[1] = 1;
kenjiArai 0:86a17116e8be 283 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 5:cf33bcfe976c 284 WAIT_MS(1); // Do I need to wait?
kenjiArai 0:86a17116e8be 285 dt[0] = BNO055_TEMP;
kenjiArai 0:86a17116e8be 286 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 287 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:86a17116e8be 288 if (c_or_f) {
AlexanderLill 7:f0eb18503c27 289 result->gyr_chip = (int8_t)dt[0] * 2;
kenjiArai 0:86a17116e8be 290 } else {
AlexanderLill 7:f0eb18503c27 291 result->gyr_chip = (int8_t)dt[0];
kenjiArai 0:86a17116e8be 292 }
kenjiArai 0:86a17116e8be 293 }
kenjiArai 0:86a17116e8be 294
kenjiArai 0:86a17116e8be 295 /////////////// Initialize ////////////////////////////////
kenjiArai 0:86a17116e8be 296 void BNO055::initialize (void)
kenjiArai 0:86a17116e8be 297 {
kenjiArai 1:cb7e19c0a702 298 #if defined(TARGET_STM32L152RE)
kenjiArai 1:cb7e19c0a702 299 _i2c.frequency(100000);
kenjiArai 1:cb7e19c0a702 300 #else
kenjiArai 1:cb7e19c0a702 301 _i2c.frequency(400000);
kenjiArai 1:cb7e19c0a702 302 #endif
kenjiArai 3:0ad6f85b178f 303 page_flag = 0xff;
kenjiArai 3:0ad6f85b178f 304 select_page(0);
kenjiArai 0:86a17116e8be 305 // Check Acc & Mag & Gyro are available of not
kenjiArai 0:86a17116e8be 306 check_id();
kenjiArai 0:86a17116e8be 307 // Set initial data
kenjiArai 0:86a17116e8be 308 set_initial_dt_to_regs();
kenjiArai 0:86a17116e8be 309 // Unit selection
kenjiArai 0:86a17116e8be 310 unit_selection();
kenjiArai 0:86a17116e8be 311 // Set fusion mode
kenjiArai 0:86a17116e8be 312 change_fusion_mode(chip_mode);
kenjiArai 0:86a17116e8be 313 }
kenjiArai 0:86a17116e8be 314
kenjiArai 0:86a17116e8be 315 void BNO055::unit_selection(void)
kenjiArai 0:86a17116e8be 316 {
kenjiArai 0:86a17116e8be 317 select_page(0);
kenjiArai 0:86a17116e8be 318 dt[0] = BNO055_UNIT_SEL;
kenjiArai 0:86a17116e8be 319 dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C;
kenjiArai 0:86a17116e8be 320 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:86a17116e8be 321 }
kenjiArai 0:86a17116e8be 322
kenjiArai 0:86a17116e8be 323 uint8_t BNO055::select_page(uint8_t page)
kenjiArai 0:86a17116e8be 324 {
kenjiArai 3:0ad6f85b178f 325 if (page != page_flag){
kenjiArai 3:0ad6f85b178f 326 dt[0] = BNO055_PAGE_ID;
kenjiArai 3:0ad6f85b178f 327 if (page == 1) {
kenjiArai 3:0ad6f85b178f 328 dt[1] = 1; // select page 1
kenjiArai 3:0ad6f85b178f 329 } else {
kenjiArai 3:0ad6f85b178f 330 dt[1] = 0; // select page 0
kenjiArai 3:0ad6f85b178f 331 }
kenjiArai 3:0ad6f85b178f 332 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 3:0ad6f85b178f 333 dt[0] = BNO055_PAGE_ID;
kenjiArai 3:0ad6f85b178f 334 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 3:0ad6f85b178f 335 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 3:0ad6f85b178f 336 page_flag = dt[0];
kenjiArai 0:86a17116e8be 337 }
kenjiArai 3:0ad6f85b178f 338 return page_flag;
kenjiArai 0:86a17116e8be 339 }
kenjiArai 0:86a17116e8be 340
kenjiArai 1:cb7e19c0a702 341 uint8_t BNO055::reset(void)
kenjiArai 1:cb7e19c0a702 342 {
kenjiArai 1:cb7e19c0a702 343 _res = 0;
kenjiArai 5:cf33bcfe976c 344 WAIT_MS(1); // Reset 1mS
kenjiArai 1:cb7e19c0a702 345 _res = 1;
kenjiArai 5:cf33bcfe976c 346 WAIT_MS(700); // Need to wait at least 650mS
kenjiArai 1:cb7e19c0a702 347 #if defined(TARGET_STM32L152RE)
kenjiArai 1:cb7e19c0a702 348 _i2c.frequency(400000);
kenjiArai 1:cb7e19c0a702 349 #else
kenjiArai 1:cb7e19c0a702 350 _i2c.frequency(400000);
kenjiArai 1:cb7e19c0a702 351 #endif
kenjiArai 1:cb7e19c0a702 352 _i2c.stop();
kenjiArai 3:0ad6f85b178f 353 page_flag = 0xff;
kenjiArai 3:0ad6f85b178f 354 select_page(0);
kenjiArai 1:cb7e19c0a702 355 check_id();
kenjiArai 1:cb7e19c0a702 356 if (chip_id != I_AM_BNO055_CHIP){
kenjiArai 1:cb7e19c0a702 357 return 1;
kenjiArai 1:cb7e19c0a702 358 } else {
kenjiArai 1:cb7e19c0a702 359 initialize();
kenjiArai 1:cb7e19c0a702 360 return 0;
kenjiArai 1:cb7e19c0a702 361 }
kenjiArai 1:cb7e19c0a702 362 }
kenjiArai 1:cb7e19c0a702 363
kenjiArai 0:86a17116e8be 364 ////// Set initialize data to related registers ///////////
kenjiArai 0:86a17116e8be 365 void BNO055::set_initial_dt_to_regs(void)
kenjiArai 0:86a17116e8be 366 {
kenjiArai 0:86a17116e8be 367 // select_page(0);
kenjiArai 0:86a17116e8be 368 // current setting is only used default values
kenjiArai 0:86a17116e8be 369 }
kenjiArai 0:86a17116e8be 370
kenjiArai 0:86a17116e8be 371 /////////////// Check Who am I? ///////////////////////////
kenjiArai 0:86a17116e8be 372 void BNO055::check_id(void)
kenjiArai 0:86a17116e8be 373 {
kenjiArai 0:86a17116e8be 374 select_page(0);
kenjiArai 0:86a17116e8be 375 // ID
kenjiArai 0:86a17116e8be 376 dt[0] = BNO055_CHIP_ID;
kenjiArai 0:86a17116e8be 377 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 378 _i2c.read(chip_addr, dt, 7, false);
kenjiArai 0:86a17116e8be 379 chip_id = dt[0];
kenjiArai 0:86a17116e8be 380 if (chip_id == I_AM_BNO055_CHIP) {
kenjiArai 3:0ad6f85b178f 381 ready_flag = 1;
kenjiArai 0:86a17116e8be 382 } else {
kenjiArai 3:0ad6f85b178f 383 ready_flag = 0;
kenjiArai 0:86a17116e8be 384 }
kenjiArai 0:86a17116e8be 385 acc_id = dt[1];
kenjiArai 0:86a17116e8be 386 if (acc_id == I_AM_BNO055_ACC) {
kenjiArai 3:0ad6f85b178f 387 ready_flag |= 2;
kenjiArai 0:86a17116e8be 388 }
kenjiArai 0:86a17116e8be 389 mag_id = dt[2];
kenjiArai 0:86a17116e8be 390 if (mag_id == I_AM_BNO055_MAG) {
kenjiArai 3:0ad6f85b178f 391 ready_flag |= 4;
kenjiArai 0:86a17116e8be 392 }
kenjiArai 0:86a17116e8be 393 gyr_id = dt[3];
kenjiArai 0:86a17116e8be 394 if (mag_id == I_AM_BNO055_MAG) {
kenjiArai 3:0ad6f85b178f 395 ready_flag |= 8;
kenjiArai 0:86a17116e8be 396 }
kenjiArai 0:86a17116e8be 397 bootldr_rev_id = dt[5]<< 8 | dt[4];
kenjiArai 0:86a17116e8be 398 sw_rev_id = dt[6];
kenjiArai 0:86a17116e8be 399 }
kenjiArai 0:86a17116e8be 400
kenjiArai 0:86a17116e8be 401 void BNO055::read_id_inf(BNO055_ID_INF_TypeDef *id)
kenjiArai 0:86a17116e8be 402 {
kenjiArai 0:86a17116e8be 403 id->chip_id = chip_id;
kenjiArai 0:86a17116e8be 404 id->acc_id = acc_id;
kenjiArai 0:86a17116e8be 405 id->mag_id = mag_id;
kenjiArai 0:86a17116e8be 406 id->gyr_id = gyr_id;
kenjiArai 0:86a17116e8be 407 id->bootldr_rev_id = bootldr_rev_id;
kenjiArai 0:86a17116e8be 408 id->sw_rev_id = sw_rev_id;
kenjiArai 0:86a17116e8be 409 }
kenjiArai 0:86a17116e8be 410
kenjiArai 0:86a17116e8be 411 /////////////// Check chip ready or not //////////////////
kenjiArai 2:0f225b686cd5 412 uint8_t BNO055::chip_ready(void)
kenjiArai 0:86a17116e8be 413 {
kenjiArai 3:0ad6f85b178f 414 if (ready_flag == 0x0f) {
kenjiArai 0:86a17116e8be 415 return 1;
kenjiArai 0:86a17116e8be 416 }
kenjiArai 0:86a17116e8be 417 return 0;
kenjiArai 0:86a17116e8be 418 }
kenjiArai 0:86a17116e8be 419
kenjiArai 2:0f225b686cd5 420 /////////////// Read Calibration status //////////////////
kenjiArai 2:0f225b686cd5 421 uint8_t BNO055::read_calib_status(void)
kenjiArai 2:0f225b686cd5 422 {
kenjiArai 2:0f225b686cd5 423 select_page(0);
kenjiArai 2:0f225b686cd5 424 dt[0] = BNO055_CALIB_STAT;
kenjiArai 2:0f225b686cd5 425 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 2:0f225b686cd5 426 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 2:0f225b686cd5 427 return dt[0];
kenjiArai 2:0f225b686cd5 428 }
kenjiArai 2:0f225b686cd5 429
kenjiArai 0:86a17116e8be 430 /////////////// Change Fusion mode ///////////////////////
kenjiArai 0:86a17116e8be 431 void BNO055::change_fusion_mode(uint8_t mode)
kenjiArai 0:86a17116e8be 432 {
kenjiArai 0:86a17116e8be 433 uint8_t current_mode;
kenjiArai 0:86a17116e8be 434
kenjiArai 0:86a17116e8be 435 select_page(0);
kenjiArai 0:86a17116e8be 436 current_mode = check_operating_mode();
kenjiArai 0:86a17116e8be 437 switch (mode) {
kenjiArai 0:86a17116e8be 438 case CONFIGMODE:
kenjiArai 0:86a17116e8be 439 dt[0] = BNO055_OPR_MODE;
kenjiArai 0:86a17116e8be 440 dt[1] = mode;
kenjiArai 0:86a17116e8be 441 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 5:cf33bcfe976c 442 WAIT_MS(19); // wait 19mS
kenjiArai 0:86a17116e8be 443 break;
kenjiArai 0:86a17116e8be 444 case MODE_IMU:
kenjiArai 0:86a17116e8be 445 case MODE_COMPASS:
kenjiArai 0:86a17116e8be 446 case MODE_M4G:
kenjiArai 0:86a17116e8be 447 case MODE_NDOF_FMC_OFF:
kenjiArai 0:86a17116e8be 448 case MODE_NDOF:
kenjiArai 0:86a17116e8be 449 if (current_mode != CONFIGMODE) { // Can we change the mode directry?
kenjiArai 0:86a17116e8be 450 dt[0] = BNO055_OPR_MODE;
kenjiArai 0:86a17116e8be 451 dt[1] = CONFIGMODE;
kenjiArai 0:86a17116e8be 452 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 5:cf33bcfe976c 453 WAIT_MS(19); // wait 19mS
kenjiArai 0:86a17116e8be 454 }
kenjiArai 0:86a17116e8be 455 dt[0] = BNO055_OPR_MODE;
kenjiArai 0:86a17116e8be 456 dt[1] = mode;
kenjiArai 0:86a17116e8be 457 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 5:cf33bcfe976c 458 WAIT_MS(7); // wait 7mS
kenjiArai 0:86a17116e8be 459 break;
kenjiArai 0:86a17116e8be 460 default:
kenjiArai 0:86a17116e8be 461 break;
kenjiArai 0:86a17116e8be 462 }
kenjiArai 0:86a17116e8be 463 }
kenjiArai 0:86a17116e8be 464
kenjiArai 0:86a17116e8be 465 uint8_t BNO055::check_operating_mode(void)
kenjiArai 0:86a17116e8be 466 {
kenjiArai 0:86a17116e8be 467 select_page(0);
kenjiArai 0:86a17116e8be 468 dt[0] = BNO055_OPR_MODE;
kenjiArai 0:86a17116e8be 469 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 470 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:86a17116e8be 471 return dt[0];
kenjiArai 0:86a17116e8be 472 }
kenjiArai 0:86a17116e8be 473
kenjiArai 0:86a17116e8be 474 /////////////// Set Mouting position /////////////////////
kenjiArai 0:86a17116e8be 475 void BNO055::set_mounting_position(uint8_t position)
kenjiArai 0:86a17116e8be 476 {
kenjiArai 0:86a17116e8be 477 uint8_t remap_config;
kenjiArai 0:86a17116e8be 478 uint8_t remap_sign;
kenjiArai 0:86a17116e8be 479 uint8_t current_mode;
kenjiArai 0:86a17116e8be 480
kenjiArai 0:86a17116e8be 481 current_mode = check_operating_mode();
kenjiArai 0:86a17116e8be 482 change_fusion_mode(CONFIGMODE);
kenjiArai 0:86a17116e8be 483 switch (position) {
kenjiArai 0:86a17116e8be 484 case MT_P0:
kenjiArai 0:86a17116e8be 485 remap_config = 0x21;
kenjiArai 0:86a17116e8be 486 remap_sign = 0x04;
kenjiArai 0:86a17116e8be 487 break;
kenjiArai 0:86a17116e8be 488 case MT_P2:
kenjiArai 0:86a17116e8be 489 remap_config = 0x24;
kenjiArai 0:86a17116e8be 490 remap_sign = 0x06;
kenjiArai 0:86a17116e8be 491 break;
kenjiArai 0:86a17116e8be 492 case MT_P3:
kenjiArai 0:86a17116e8be 493 remap_config = 0x21;
kenjiArai 0:86a17116e8be 494 remap_sign = 0x02;
kenjiArai 0:86a17116e8be 495 break;
kenjiArai 0:86a17116e8be 496 case MT_P4:
kenjiArai 0:86a17116e8be 497 remap_config = 0x24;
kenjiArai 0:86a17116e8be 498 remap_sign = 0x03;
kenjiArai 0:86a17116e8be 499 break;
kenjiArai 0:86a17116e8be 500 case MT_P5:
kenjiArai 0:86a17116e8be 501 remap_config = 0x21;
kenjiArai 0:86a17116e8be 502 remap_sign = 0x01;
kenjiArai 0:86a17116e8be 503 break;
kenjiArai 0:86a17116e8be 504 case MT_P6:
kenjiArai 0:86a17116e8be 505 remap_config = 0x21;
kenjiArai 0:86a17116e8be 506 remap_sign = 0x07;
kenjiArai 0:86a17116e8be 507 break;
kenjiArai 0:86a17116e8be 508 case MT_P7:
kenjiArai 0:86a17116e8be 509 remap_config = 0x24;
kenjiArai 0:86a17116e8be 510 remap_sign = 0x05;
kenjiArai 0:86a17116e8be 511 break;
kenjiArai 0:86a17116e8be 512 case MT_P1:
kenjiArai 0:86a17116e8be 513 default:
kenjiArai 0:86a17116e8be 514 remap_config = 0x24;
kenjiArai 0:86a17116e8be 515 remap_sign = 0x00;
kenjiArai 0:86a17116e8be 516 break;
kenjiArai 0:86a17116e8be 517 }
kenjiArai 0:86a17116e8be 518 dt[0] = BNO055_AXIS_MAP_CONFIG;
kenjiArai 0:86a17116e8be 519 dt[1] = remap_config;
kenjiArai 0:86a17116e8be 520 dt[2] = remap_sign;
kenjiArai 0:86a17116e8be 521 _i2c.write(chip_addr, dt, 3, false);
kenjiArai 0:86a17116e8be 522 change_fusion_mode(current_mode);
kenjiArai 0:86a17116e8be 523 }
kenjiArai 0:86a17116e8be 524
kenjiArai 0:86a17116e8be 525 /////////////// I2C Freq. /////////////////////////////////
kenjiArai 0:86a17116e8be 526 void BNO055::frequency(int hz)
kenjiArai 0:86a17116e8be 527 {
kenjiArai 0:86a17116e8be 528 _i2c.frequency(hz);
kenjiArai 0:86a17116e8be 529 }
kenjiArai 0:86a17116e8be 530
kenjiArai 0:86a17116e8be 531 /////////////// Read/Write specific register //////////////
kenjiArai 0:86a17116e8be 532 uint8_t BNO055::read_reg0(uint8_t addr)
kenjiArai 0:86a17116e8be 533 {
kenjiArai 0:86a17116e8be 534 select_page(0);
kenjiArai 0:86a17116e8be 535 dt[0] = addr;
kenjiArai 0:86a17116e8be 536 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 537 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:86a17116e8be 538 return (uint8_t)dt[0];
kenjiArai 0:86a17116e8be 539 }
kenjiArai 0:86a17116e8be 540
kenjiArai 0:86a17116e8be 541 uint8_t BNO055::write_reg0(uint8_t addr, uint8_t data)
kenjiArai 0:86a17116e8be 542 {
kenjiArai 0:86a17116e8be 543 uint8_t current_mode;
kenjiArai 0:86a17116e8be 544 uint8_t d;
kenjiArai 0:86a17116e8be 545
kenjiArai 0:86a17116e8be 546 current_mode = check_operating_mode();
kenjiArai 0:86a17116e8be 547 change_fusion_mode(CONFIGMODE);
kenjiArai 0:86a17116e8be 548 dt[0] = addr;
kenjiArai 0:86a17116e8be 549 dt[1] = data;
kenjiArai 0:86a17116e8be 550 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:86a17116e8be 551 d = dt[0];
kenjiArai 0:86a17116e8be 552 change_fusion_mode(current_mode);
kenjiArai 0:86a17116e8be 553 return d;
kenjiArai 0:86a17116e8be 554 }
kenjiArai 0:86a17116e8be 555
kenjiArai 0:86a17116e8be 556 uint8_t BNO055::read_reg1(uint8_t addr)
kenjiArai 0:86a17116e8be 557 {
kenjiArai 0:86a17116e8be 558 select_page(1);
kenjiArai 0:86a17116e8be 559 dt[0] = addr;
kenjiArai 0:86a17116e8be 560 _i2c.write(chip_addr, dt, 1, true);
kenjiArai 0:86a17116e8be 561 _i2c.read(chip_addr, dt, 1, false);
kenjiArai 0:86a17116e8be 562 return (uint8_t)dt[0];
kenjiArai 0:86a17116e8be 563 }
kenjiArai 0:86a17116e8be 564
kenjiArai 0:86a17116e8be 565 uint8_t BNO055::write_reg1(uint8_t addr, uint8_t data)
kenjiArai 0:86a17116e8be 566 {
kenjiArai 0:86a17116e8be 567 uint8_t current_mode;
kenjiArai 0:86a17116e8be 568 uint8_t d;
kenjiArai 0:86a17116e8be 569
kenjiArai 0:86a17116e8be 570 current_mode = check_operating_mode();
kenjiArai 0:86a17116e8be 571 change_fusion_mode(CONFIGMODE);
kenjiArai 0:86a17116e8be 572 select_page(1);
kenjiArai 0:86a17116e8be 573 dt[0] = addr;
kenjiArai 0:86a17116e8be 574 dt[1] = data;
kenjiArai 0:86a17116e8be 575 _i2c.write(chip_addr, dt, 2, false);
kenjiArai 0:86a17116e8be 576 d = dt[0];
kenjiArai 0:86a17116e8be 577 change_fusion_mode(current_mode);
kenjiArai 0:86a17116e8be 578 return d;
kenjiArai 0:86a17116e8be 579 }