Kenji Arai / BNO055_fusion

BNO055 Intelligent 9-axis absolute orientation sensor by Bosch Sensortec. It includes ACC, MAG and GYRO sensors and Cortex-M0+ processor.

Dependents:   BNO055_test BNO055-ELEC3810 1BNO055 DEMO3 ... more

Please see follows.
/users/kenjiArai/notebook/bno055---orientation-sensor/#

BNO055.cpp@7:b48d96169302, 2020-08-05 (annotated)

Committer:
kenjiArai
Date:
Wed Aug 05 05:28:13 2020 +0000
Revision:
7:b48d96169302
Parent:
6:07d01bf36ad0 
run on mbed-os6.2.0

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