Dan Perrett / BNO055_fusion

Fork of Alexander Lill's BNO055_fusion library

BNO055.cpp@10:63a9849f0e97, 2018-01-28 (annotated)

Committer:
AlexanderLill
Date:
Sun Jan 28 20:55:10 2018 +0000
Revision:
10:63a9849f0e97
Parent:
9:e7c8d34bf79a
Child:
11:17bc36c5ccbb
Refactoring: Create functions to check for selected unit

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