Alexander Lill / BNO055_fusion

Extended and refactored library for BNO055, an intelligent 9-axis absolute orientation sensor by Bosch Sensortec. It includes ACC, MAG and GYRO sensors and Cortex-M0 processor.

Fork of BNO055_fusion by Kenji Arai

Please note: pitch and roll in get_euler_angles are switched, the code should be like this:

h = dt[1] << 8 | dt[0]; r = dt[3] << 8 | dt[2]; p = dt[5] << 8 | dt[4];

See https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf

BNO055.cpp@14:9f9a12bcd8ed, 2021-03-05 (annotated)

Committer:
danmbed
Date:
Fri Mar 05 13:58:00 2021 +0000
Revision:
14:9f9a12bcd8ed
Parent:
13:92854c8deb3c 
roll and pitch Euler angles corrected

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