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?
User | Revision | Line number | New 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 | } |