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