Darren Ulrich / BNO055_fusion

Forked NO055 IMU No Changes From Origional

Fork of BNO055_fusion by Kenji Arai

BNO055.cpp@0:86a17116e8be, 2015-04-05 (annotated)

Committer:
kenjiArai
Date:
Sun Apr 05 04:12:58 2015 +0000
Revision:
0:86a17116e8be
Child:
1:cb7e19c0a702
BNO055 Intelligent 9-axis absolute orientation sensor by Bosch Sensortec.; It includes ACC, MAG and GYRO sensors and Cortex-M0+ processor.

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