Dan Perrett / BNO055_fusion

Fork of Alexander Lill's BNO055_fusion library

BNO055.cpp@11:17bc36c5ccbb, 2018-01-28 (annotated)

Committer:
AlexanderLill
Date:
Sun Jan 28 21:29:12 2018 +0000
Revision:
11:17bc36c5ccbb
Parent:
10:63a9849f0e97
Child:
13:92854c8deb3c
Fix data type and conversion for quaternions

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