same
Fork of BNO055_fusion by
BNO055.cpp@2:0f225b686cd5, 2015-04-08 (annotated)
- Committer:
- kenjiArai
- Date:
- Wed Apr 08 11:27:57 2015 +0000
- Revision:
- 2:0f225b686cd5
- Parent:
- 1:cb7e19c0a702
- Child:
- 3:0ad6f85b178f
Added read_calib_status() function
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 | 2:0f225b686cd5 | 10 | * Revised: April 8th, 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 | 0:86a17116e8be | 76 | el->h = (double)(h / 900); |
kenjiArai | 0:86a17116e8be | 77 | el->p = (double)(p / 900); |
kenjiArai | 0:86a17116e8be | 78 | el->r = (double)(r / 900); |
kenjiArai | 0:86a17116e8be | 79 | } else { |
kenjiArai | 0:86a17116e8be | 80 | el->h = (double)(h / 16); |
kenjiArai | 0:86a17116e8be | 81 | el->p = (double)(p / 16); |
kenjiArai | 0:86a17116e8be | 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 | |
kenjiArai | 0:86a17116e8be | 98 | void BNO055::get_linear_accel(BNO055_LIN_ACC_TypeDef *la) |
kenjiArai | 0:86a17116e8be | 99 | { |
kenjiArai | 0:86a17116e8be | 100 | uint8_t ms2_or_mg; |
kenjiArai | 0:86a17116e8be | 101 | int16_t x,y,z; |
kenjiArai | 0:86a17116e8be | 102 | |
kenjiArai | 0:86a17116e8be | 103 | select_page(0); |
kenjiArai | 0:86a17116e8be | 104 | dt[0] = BNO055_UNIT_SEL; |
kenjiArai | 0:86a17116e8be | 105 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 106 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:86a17116e8be | 107 | if (dt[0] & 0x01) { |
kenjiArai | 0:86a17116e8be | 108 | ms2_or_mg = 1; // mg |
kenjiArai | 0:86a17116e8be | 109 | } else { |
kenjiArai | 0:86a17116e8be | 110 | ms2_or_mg = 0; // m/s*s |
kenjiArai | 0:86a17116e8be | 111 | } |
kenjiArai | 0:86a17116e8be | 112 | dt[0] = BNO055_LINEAR_ACC_X_LSB; |
kenjiArai | 0:86a17116e8be | 113 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 114 | _i2c.read(chip_addr, dt, 6, false); |
kenjiArai | 0:86a17116e8be | 115 | x = dt[1] << 8 | dt[0]; |
kenjiArai | 0:86a17116e8be | 116 | y = dt[3] << 8 | dt[2]; |
kenjiArai | 0:86a17116e8be | 117 | z = dt[5] << 8 | dt[4]; |
kenjiArai | 0:86a17116e8be | 118 | if (ms2_or_mg) { |
kenjiArai | 0:86a17116e8be | 119 | la->x = (double)x; |
kenjiArai | 0:86a17116e8be | 120 | la->y = (double)y; |
kenjiArai | 0:86a17116e8be | 121 | la->z = (double)z; |
kenjiArai | 0:86a17116e8be | 122 | } else { |
kenjiArai | 0:86a17116e8be | 123 | la->x = (double)(x / 100); |
kenjiArai | 0:86a17116e8be | 124 | la->y = (double)(y / 100); |
kenjiArai | 0:86a17116e8be | 125 | la->z = (double)(z / 100); |
kenjiArai | 0:86a17116e8be | 126 | } |
kenjiArai | 0:86a17116e8be | 127 | } |
kenjiArai | 0:86a17116e8be | 128 | |
kenjiArai | 0:86a17116e8be | 129 | void BNO055::get_gravity(BNO055_GRAVITY_TypeDef *gr) |
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_GRAVITY_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 | gr->x = (double)x; |
kenjiArai | 0:86a17116e8be | 151 | gr->y = (double)y; |
kenjiArai | 0:86a17116e8be | 152 | gr->z = (double)z; |
kenjiArai | 0:86a17116e8be | 153 | } else { |
kenjiArai | 0:86a17116e8be | 154 | gr->x = (double)(x / 100); |
kenjiArai | 0:86a17116e8be | 155 | gr->y = (double)(y / 100); |
kenjiArai | 0:86a17116e8be | 156 | gr->z = (double)(z / 100); |
kenjiArai | 0:86a17116e8be | 157 | } |
kenjiArai | 0:86a17116e8be | 158 | } |
kenjiArai | 0:86a17116e8be | 159 | |
kenjiArai | 0:86a17116e8be | 160 | void BNO055::get_chip_temperature(BNO055_TEMPERATURE_TypeDef *tmp) |
kenjiArai | 0:86a17116e8be | 161 | { |
kenjiArai | 0:86a17116e8be | 162 | uint8_t c_or_f; |
kenjiArai | 0:86a17116e8be | 163 | |
kenjiArai | 0:86a17116e8be | 164 | select_page(0); |
kenjiArai | 0:86a17116e8be | 165 | dt[0] = BNO055_UNIT_SEL; |
kenjiArai | 0:86a17116e8be | 166 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 167 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:86a17116e8be | 168 | if (dt[0] & 0x10) { |
kenjiArai | 0:86a17116e8be | 169 | c_or_f = 1; // Fahrenheit |
kenjiArai | 0:86a17116e8be | 170 | } else { |
kenjiArai | 0:86a17116e8be | 171 | c_or_f = 0; // degrees Celsius |
kenjiArai | 0:86a17116e8be | 172 | } |
kenjiArai | 0:86a17116e8be | 173 | dt[0] = BNO055_TEMP_SOURCE; |
kenjiArai | 0:86a17116e8be | 174 | dt[1] = 0; |
kenjiArai | 0:86a17116e8be | 175 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:86a17116e8be | 176 | wait_ms(1); // Do I need to wait? |
kenjiArai | 0:86a17116e8be | 177 | dt[0] = BNO055_TEMP; |
kenjiArai | 0:86a17116e8be | 178 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 179 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:86a17116e8be | 180 | if (c_or_f) { |
kenjiArai | 0:86a17116e8be | 181 | tmp->acc_chip = (int8_t)dt[0] * 2; |
kenjiArai | 0:86a17116e8be | 182 | } else { |
kenjiArai | 0:86a17116e8be | 183 | tmp->acc_chip = (int8_t)dt[0]; |
kenjiArai | 0:86a17116e8be | 184 | } |
kenjiArai | 0:86a17116e8be | 185 | dt[0] = BNO055_TEMP_SOURCE; |
kenjiArai | 0:86a17116e8be | 186 | dt[1] = 1; |
kenjiArai | 0:86a17116e8be | 187 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:86a17116e8be | 188 | wait_ms(1); // Do I need to wait? |
kenjiArai | 0:86a17116e8be | 189 | dt[0] = BNO055_TEMP; |
kenjiArai | 0:86a17116e8be | 190 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 191 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:86a17116e8be | 192 | if (c_or_f) { |
kenjiArai | 0:86a17116e8be | 193 | tmp->gyr_chip = (int8_t)dt[0] * 2; |
kenjiArai | 0:86a17116e8be | 194 | } else { |
kenjiArai | 0:86a17116e8be | 195 | tmp->gyr_chip = (int8_t)dt[0]; |
kenjiArai | 0:86a17116e8be | 196 | } |
kenjiArai | 0:86a17116e8be | 197 | } |
kenjiArai | 0:86a17116e8be | 198 | |
kenjiArai | 0:86a17116e8be | 199 | /////////////// Initialize //////////////////////////////// |
kenjiArai | 0:86a17116e8be | 200 | void BNO055::initialize (void) |
kenjiArai | 0:86a17116e8be | 201 | { |
kenjiArai | 1:cb7e19c0a702 | 202 | #if defined(TARGET_STM32L152RE) |
kenjiArai | 1:cb7e19c0a702 | 203 | _i2c.frequency(100000); |
kenjiArai | 1:cb7e19c0a702 | 204 | #else |
kenjiArai | 1:cb7e19c0a702 | 205 | _i2c.frequency(400000); |
kenjiArai | 1:cb7e19c0a702 | 206 | #endif |
kenjiArai | 0:86a17116e8be | 207 | // Check Acc & Mag & Gyro are available of not |
kenjiArai | 0:86a17116e8be | 208 | check_id(); |
kenjiArai | 0:86a17116e8be | 209 | // Set initial data |
kenjiArai | 0:86a17116e8be | 210 | set_initial_dt_to_regs(); |
kenjiArai | 0:86a17116e8be | 211 | // Unit selection |
kenjiArai | 0:86a17116e8be | 212 | unit_selection(); |
kenjiArai | 0:86a17116e8be | 213 | // Set fusion mode |
kenjiArai | 0:86a17116e8be | 214 | change_fusion_mode(chip_mode); |
kenjiArai | 0:86a17116e8be | 215 | } |
kenjiArai | 0:86a17116e8be | 216 | |
kenjiArai | 0:86a17116e8be | 217 | void BNO055::unit_selection(void) |
kenjiArai | 0:86a17116e8be | 218 | { |
kenjiArai | 0:86a17116e8be | 219 | select_page(0); |
kenjiArai | 0:86a17116e8be | 220 | dt[0] = BNO055_UNIT_SEL; |
kenjiArai | 0:86a17116e8be | 221 | dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C; |
kenjiArai | 0:86a17116e8be | 222 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:86a17116e8be | 223 | } |
kenjiArai | 0:86a17116e8be | 224 | |
kenjiArai | 0:86a17116e8be | 225 | uint8_t BNO055::select_page(uint8_t page) |
kenjiArai | 0:86a17116e8be | 226 | { |
kenjiArai | 0:86a17116e8be | 227 | dt[0] = BNO055_PAGE_ID; |
kenjiArai | 0:86a17116e8be | 228 | if (page == 1) { |
kenjiArai | 0:86a17116e8be | 229 | dt[1] = 1; // select page 1 |
kenjiArai | 0:86a17116e8be | 230 | } else { |
kenjiArai | 0:86a17116e8be | 231 | dt[1] = 0; // select page 0 |
kenjiArai | 0:86a17116e8be | 232 | } |
kenjiArai | 0:86a17116e8be | 233 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:86a17116e8be | 234 | dt[0] = BNO055_PAGE_ID; |
kenjiArai | 0:86a17116e8be | 235 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 236 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:86a17116e8be | 237 | return dt[0]; |
kenjiArai | 0:86a17116e8be | 238 | } |
kenjiArai | 0:86a17116e8be | 239 | |
kenjiArai | 1:cb7e19c0a702 | 240 | uint8_t BNO055::reset(void) |
kenjiArai | 1:cb7e19c0a702 | 241 | { |
kenjiArai | 1:cb7e19c0a702 | 242 | _res = 0; |
kenjiArai | 1:cb7e19c0a702 | 243 | wait_ms(1); // Reset 1mS |
kenjiArai | 1:cb7e19c0a702 | 244 | _res = 1; |
kenjiArai | 1:cb7e19c0a702 | 245 | wait(0.7); // Need to wait at least 650mS |
kenjiArai | 1:cb7e19c0a702 | 246 | #if defined(TARGET_STM32L152RE) |
kenjiArai | 1:cb7e19c0a702 | 247 | _i2c.frequency(400000); |
kenjiArai | 1:cb7e19c0a702 | 248 | #else |
kenjiArai | 1:cb7e19c0a702 | 249 | _i2c.frequency(400000); |
kenjiArai | 1:cb7e19c0a702 | 250 | #endif |
kenjiArai | 1:cb7e19c0a702 | 251 | _i2c.stop(); |
kenjiArai | 1:cb7e19c0a702 | 252 | check_id(); |
kenjiArai | 1:cb7e19c0a702 | 253 | if (chip_id != I_AM_BNO055_CHIP){ |
kenjiArai | 1:cb7e19c0a702 | 254 | return 1; |
kenjiArai | 1:cb7e19c0a702 | 255 | } else { |
kenjiArai | 1:cb7e19c0a702 | 256 | initialize(); |
kenjiArai | 1:cb7e19c0a702 | 257 | return 0; |
kenjiArai | 1:cb7e19c0a702 | 258 | } |
kenjiArai | 1:cb7e19c0a702 | 259 | } |
kenjiArai | 1:cb7e19c0a702 | 260 | |
kenjiArai | 0:86a17116e8be | 261 | ////// Set initialize data to related registers /////////// |
kenjiArai | 0:86a17116e8be | 262 | void BNO055::set_initial_dt_to_regs(void) |
kenjiArai | 0:86a17116e8be | 263 | { |
kenjiArai | 0:86a17116e8be | 264 | // select_page(0); |
kenjiArai | 0:86a17116e8be | 265 | // current setting is only used default values |
kenjiArai | 0:86a17116e8be | 266 | } |
kenjiArai | 0:86a17116e8be | 267 | |
kenjiArai | 0:86a17116e8be | 268 | /////////////// Check Who am I? /////////////////////////// |
kenjiArai | 0:86a17116e8be | 269 | void BNO055::check_id(void) |
kenjiArai | 0:86a17116e8be | 270 | { |
kenjiArai | 0:86a17116e8be | 271 | select_page(0); |
kenjiArai | 0:86a17116e8be | 272 | // ID |
kenjiArai | 0:86a17116e8be | 273 | dt[0] = BNO055_CHIP_ID; |
kenjiArai | 0:86a17116e8be | 274 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 275 | _i2c.read(chip_addr, dt, 7, false); |
kenjiArai | 0:86a17116e8be | 276 | chip_id = dt[0]; |
kenjiArai | 0:86a17116e8be | 277 | if (chip_id == I_AM_BNO055_CHIP) { |
kenjiArai | 0:86a17116e8be | 278 | ready_flg = 1; |
kenjiArai | 0:86a17116e8be | 279 | } else { |
kenjiArai | 0:86a17116e8be | 280 | ready_flg = 0; |
kenjiArai | 0:86a17116e8be | 281 | } |
kenjiArai | 0:86a17116e8be | 282 | acc_id = dt[1]; |
kenjiArai | 0:86a17116e8be | 283 | if (acc_id == I_AM_BNO055_ACC) { |
kenjiArai | 0:86a17116e8be | 284 | ready_flg |= 2; |
kenjiArai | 0:86a17116e8be | 285 | } |
kenjiArai | 0:86a17116e8be | 286 | mag_id = dt[2]; |
kenjiArai | 0:86a17116e8be | 287 | if (mag_id == I_AM_BNO055_MAG) { |
kenjiArai | 0:86a17116e8be | 288 | ready_flg |= 4; |
kenjiArai | 0:86a17116e8be | 289 | } |
kenjiArai | 0:86a17116e8be | 290 | gyr_id = dt[3]; |
kenjiArai | 0:86a17116e8be | 291 | if (mag_id == I_AM_BNO055_MAG) { |
kenjiArai | 0:86a17116e8be | 292 | ready_flg |= 8; |
kenjiArai | 0:86a17116e8be | 293 | } |
kenjiArai | 0:86a17116e8be | 294 | bootldr_rev_id = dt[5]<< 8 | dt[4]; |
kenjiArai | 0:86a17116e8be | 295 | sw_rev_id = dt[6]; |
kenjiArai | 0:86a17116e8be | 296 | } |
kenjiArai | 0:86a17116e8be | 297 | |
kenjiArai | 0:86a17116e8be | 298 | void BNO055::read_id_inf(BNO055_ID_INF_TypeDef *id) |
kenjiArai | 0:86a17116e8be | 299 | { |
kenjiArai | 0:86a17116e8be | 300 | id->chip_id = chip_id; |
kenjiArai | 0:86a17116e8be | 301 | id->acc_id = acc_id; |
kenjiArai | 0:86a17116e8be | 302 | id->mag_id = mag_id; |
kenjiArai | 0:86a17116e8be | 303 | id->gyr_id = gyr_id; |
kenjiArai | 0:86a17116e8be | 304 | id->bootldr_rev_id = bootldr_rev_id; |
kenjiArai | 0:86a17116e8be | 305 | id->sw_rev_id = sw_rev_id; |
kenjiArai | 0:86a17116e8be | 306 | } |
kenjiArai | 0:86a17116e8be | 307 | |
kenjiArai | 0:86a17116e8be | 308 | /////////////// Check chip ready or not ////////////////// |
kenjiArai | 2:0f225b686cd5 | 309 | uint8_t BNO055::chip_ready(void) |
kenjiArai | 0:86a17116e8be | 310 | { |
kenjiArai | 0:86a17116e8be | 311 | if (ready_flg == 0x0f) { |
kenjiArai | 0:86a17116e8be | 312 | return 1; |
kenjiArai | 0:86a17116e8be | 313 | } |
kenjiArai | 0:86a17116e8be | 314 | return 0; |
kenjiArai | 0:86a17116e8be | 315 | } |
kenjiArai | 0:86a17116e8be | 316 | |
kenjiArai | 2:0f225b686cd5 | 317 | /////////////// Read Calibration status ////////////////// |
kenjiArai | 2:0f225b686cd5 | 318 | uint8_t BNO055::read_calib_status(void) |
kenjiArai | 2:0f225b686cd5 | 319 | { |
kenjiArai | 2:0f225b686cd5 | 320 | select_page(0); |
kenjiArai | 2:0f225b686cd5 | 321 | dt[0] = BNO055_CALIB_STAT; |
kenjiArai | 2:0f225b686cd5 | 322 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 2:0f225b686cd5 | 323 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 2:0f225b686cd5 | 324 | return dt[0]; |
kenjiArai | 2:0f225b686cd5 | 325 | } |
kenjiArai | 2:0f225b686cd5 | 326 | |
kenjiArai | 0:86a17116e8be | 327 | /////////////// Change Fusion mode /////////////////////// |
kenjiArai | 0:86a17116e8be | 328 | void BNO055::change_fusion_mode(uint8_t mode) |
kenjiArai | 0:86a17116e8be | 329 | { |
kenjiArai | 0:86a17116e8be | 330 | uint8_t current_mode; |
kenjiArai | 0:86a17116e8be | 331 | |
kenjiArai | 0:86a17116e8be | 332 | select_page(0); |
kenjiArai | 0:86a17116e8be | 333 | current_mode = check_operating_mode(); |
kenjiArai | 0:86a17116e8be | 334 | switch (mode) { |
kenjiArai | 0:86a17116e8be | 335 | case CONFIGMODE: |
kenjiArai | 0:86a17116e8be | 336 | dt[0] = BNO055_OPR_MODE; |
kenjiArai | 0:86a17116e8be | 337 | dt[1] = mode; |
kenjiArai | 0:86a17116e8be | 338 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:86a17116e8be | 339 | wait_ms(19); // wait 19mS |
kenjiArai | 0:86a17116e8be | 340 | break; |
kenjiArai | 0:86a17116e8be | 341 | case MODE_IMU: |
kenjiArai | 0:86a17116e8be | 342 | case MODE_COMPASS: |
kenjiArai | 0:86a17116e8be | 343 | case MODE_M4G: |
kenjiArai | 0:86a17116e8be | 344 | case MODE_NDOF_FMC_OFF: |
kenjiArai | 0:86a17116e8be | 345 | case MODE_NDOF: |
kenjiArai | 0:86a17116e8be | 346 | if (current_mode != CONFIGMODE) { // Can we change the mode directry? |
kenjiArai | 0:86a17116e8be | 347 | dt[0] = BNO055_OPR_MODE; |
kenjiArai | 0:86a17116e8be | 348 | dt[1] = CONFIGMODE; |
kenjiArai | 0:86a17116e8be | 349 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:86a17116e8be | 350 | wait_ms(19); // wait 19mS |
kenjiArai | 0:86a17116e8be | 351 | } |
kenjiArai | 0:86a17116e8be | 352 | dt[0] = BNO055_OPR_MODE; |
kenjiArai | 0:86a17116e8be | 353 | dt[1] = mode; |
kenjiArai | 0:86a17116e8be | 354 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:86a17116e8be | 355 | wait_ms(7); // wait 7mS |
kenjiArai | 0:86a17116e8be | 356 | break; |
kenjiArai | 0:86a17116e8be | 357 | default: |
kenjiArai | 0:86a17116e8be | 358 | break; |
kenjiArai | 0:86a17116e8be | 359 | } |
kenjiArai | 0:86a17116e8be | 360 | } |
kenjiArai | 0:86a17116e8be | 361 | |
kenjiArai | 0:86a17116e8be | 362 | uint8_t BNO055::check_operating_mode(void) |
kenjiArai | 0:86a17116e8be | 363 | { |
kenjiArai | 0:86a17116e8be | 364 | select_page(0); |
kenjiArai | 0:86a17116e8be | 365 | dt[0] = BNO055_OPR_MODE; |
kenjiArai | 0:86a17116e8be | 366 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 367 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:86a17116e8be | 368 | return dt[0]; |
kenjiArai | 0:86a17116e8be | 369 | } |
kenjiArai | 0:86a17116e8be | 370 | |
kenjiArai | 0:86a17116e8be | 371 | /////////////// Set Mouting position ///////////////////// |
kenjiArai | 0:86a17116e8be | 372 | void BNO055::set_mounting_position(uint8_t position) |
kenjiArai | 0:86a17116e8be | 373 | { |
kenjiArai | 0:86a17116e8be | 374 | uint8_t remap_config; |
kenjiArai | 0:86a17116e8be | 375 | uint8_t remap_sign; |
kenjiArai | 0:86a17116e8be | 376 | uint8_t current_mode; |
kenjiArai | 0:86a17116e8be | 377 | |
kenjiArai | 0:86a17116e8be | 378 | current_mode = check_operating_mode(); |
kenjiArai | 0:86a17116e8be | 379 | change_fusion_mode(CONFIGMODE); |
kenjiArai | 0:86a17116e8be | 380 | switch (position) { |
kenjiArai | 0:86a17116e8be | 381 | case MT_P0: |
kenjiArai | 0:86a17116e8be | 382 | remap_config = 0x21; |
kenjiArai | 0:86a17116e8be | 383 | remap_sign = 0x04; |
kenjiArai | 0:86a17116e8be | 384 | break; |
kenjiArai | 0:86a17116e8be | 385 | case MT_P2: |
kenjiArai | 0:86a17116e8be | 386 | remap_config = 0x24; |
kenjiArai | 0:86a17116e8be | 387 | remap_sign = 0x06; |
kenjiArai | 0:86a17116e8be | 388 | break; |
kenjiArai | 0:86a17116e8be | 389 | case MT_P3: |
kenjiArai | 0:86a17116e8be | 390 | remap_config = 0x21; |
kenjiArai | 0:86a17116e8be | 391 | remap_sign = 0x02; |
kenjiArai | 0:86a17116e8be | 392 | break; |
kenjiArai | 0:86a17116e8be | 393 | case MT_P4: |
kenjiArai | 0:86a17116e8be | 394 | remap_config = 0x24; |
kenjiArai | 0:86a17116e8be | 395 | remap_sign = 0x03; |
kenjiArai | 0:86a17116e8be | 396 | break; |
kenjiArai | 0:86a17116e8be | 397 | case MT_P5: |
kenjiArai | 0:86a17116e8be | 398 | remap_config = 0x21; |
kenjiArai | 0:86a17116e8be | 399 | remap_sign = 0x01; |
kenjiArai | 0:86a17116e8be | 400 | break; |
kenjiArai | 0:86a17116e8be | 401 | case MT_P6: |
kenjiArai | 0:86a17116e8be | 402 | remap_config = 0x21; |
kenjiArai | 0:86a17116e8be | 403 | remap_sign = 0x07; |
kenjiArai | 0:86a17116e8be | 404 | break; |
kenjiArai | 0:86a17116e8be | 405 | case MT_P7: |
kenjiArai | 0:86a17116e8be | 406 | remap_config = 0x24; |
kenjiArai | 0:86a17116e8be | 407 | remap_sign = 0x05; |
kenjiArai | 0:86a17116e8be | 408 | break; |
kenjiArai | 0:86a17116e8be | 409 | case MT_P1: |
kenjiArai | 0:86a17116e8be | 410 | default: |
kenjiArai | 0:86a17116e8be | 411 | remap_config = 0x24; |
kenjiArai | 0:86a17116e8be | 412 | remap_sign = 0x00; |
kenjiArai | 0:86a17116e8be | 413 | break; |
kenjiArai | 0:86a17116e8be | 414 | } |
kenjiArai | 0:86a17116e8be | 415 | dt[0] = BNO055_AXIS_MAP_CONFIG; |
kenjiArai | 0:86a17116e8be | 416 | dt[1] = remap_config; |
kenjiArai | 0:86a17116e8be | 417 | dt[2] = remap_sign; |
kenjiArai | 0:86a17116e8be | 418 | _i2c.write(chip_addr, dt, 3, false); |
kenjiArai | 0:86a17116e8be | 419 | change_fusion_mode(current_mode); |
kenjiArai | 0:86a17116e8be | 420 | } |
kenjiArai | 0:86a17116e8be | 421 | |
kenjiArai | 0:86a17116e8be | 422 | /////////////// I2C Freq. ///////////////////////////////// |
kenjiArai | 0:86a17116e8be | 423 | void BNO055::frequency(int hz) |
kenjiArai | 0:86a17116e8be | 424 | { |
kenjiArai | 0:86a17116e8be | 425 | _i2c.frequency(hz); |
kenjiArai | 0:86a17116e8be | 426 | } |
kenjiArai | 0:86a17116e8be | 427 | |
kenjiArai | 0:86a17116e8be | 428 | /////////////// Read/Write specific register ////////////// |
kenjiArai | 0:86a17116e8be | 429 | uint8_t BNO055::read_reg0(uint8_t addr) |
kenjiArai | 0:86a17116e8be | 430 | { |
kenjiArai | 0:86a17116e8be | 431 | select_page(0); |
kenjiArai | 0:86a17116e8be | 432 | dt[0] = addr; |
kenjiArai | 0:86a17116e8be | 433 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 434 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:86a17116e8be | 435 | return (uint8_t)dt[0]; |
kenjiArai | 0:86a17116e8be | 436 | } |
kenjiArai | 0:86a17116e8be | 437 | |
kenjiArai | 0:86a17116e8be | 438 | uint8_t BNO055::write_reg0(uint8_t addr, uint8_t data) |
kenjiArai | 0:86a17116e8be | 439 | { |
kenjiArai | 0:86a17116e8be | 440 | uint8_t current_mode; |
kenjiArai | 0:86a17116e8be | 441 | uint8_t d; |
kenjiArai | 0:86a17116e8be | 442 | |
kenjiArai | 0:86a17116e8be | 443 | current_mode = check_operating_mode(); |
kenjiArai | 0:86a17116e8be | 444 | change_fusion_mode(CONFIGMODE); |
kenjiArai | 0:86a17116e8be | 445 | dt[0] = addr; |
kenjiArai | 0:86a17116e8be | 446 | dt[1] = data; |
kenjiArai | 0:86a17116e8be | 447 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:86a17116e8be | 448 | d = dt[0]; |
kenjiArai | 0:86a17116e8be | 449 | change_fusion_mode(current_mode); |
kenjiArai | 0:86a17116e8be | 450 | return d; |
kenjiArai | 0:86a17116e8be | 451 | } |
kenjiArai | 0:86a17116e8be | 452 | |
kenjiArai | 0:86a17116e8be | 453 | uint8_t BNO055::read_reg1(uint8_t addr) |
kenjiArai | 0:86a17116e8be | 454 | { |
kenjiArai | 0:86a17116e8be | 455 | select_page(1); |
kenjiArai | 0:86a17116e8be | 456 | dt[0] = addr; |
kenjiArai | 0:86a17116e8be | 457 | _i2c.write(chip_addr, dt, 1, true); |
kenjiArai | 0:86a17116e8be | 458 | _i2c.read(chip_addr, dt, 1, false); |
kenjiArai | 0:86a17116e8be | 459 | return (uint8_t)dt[0]; |
kenjiArai | 0:86a17116e8be | 460 | } |
kenjiArai | 0:86a17116e8be | 461 | |
kenjiArai | 0:86a17116e8be | 462 | uint8_t BNO055::write_reg1(uint8_t addr, uint8_t data) |
kenjiArai | 0:86a17116e8be | 463 | { |
kenjiArai | 0:86a17116e8be | 464 | uint8_t current_mode; |
kenjiArai | 0:86a17116e8be | 465 | uint8_t d; |
kenjiArai | 0:86a17116e8be | 466 | |
kenjiArai | 0:86a17116e8be | 467 | current_mode = check_operating_mode(); |
kenjiArai | 0:86a17116e8be | 468 | change_fusion_mode(CONFIGMODE); |
kenjiArai | 0:86a17116e8be | 469 | select_page(1); |
kenjiArai | 0:86a17116e8be | 470 | dt[0] = addr; |
kenjiArai | 0:86a17116e8be | 471 | dt[1] = data; |
kenjiArai | 0:86a17116e8be | 472 | _i2c.write(chip_addr, dt, 2, false); |
kenjiArai | 0:86a17116e8be | 473 | d = dt[0]; |
kenjiArai | 0:86a17116e8be | 474 | change_fusion_mode(current_mode); |
kenjiArai | 0:86a17116e8be | 475 | return d; |
kenjiArai | 0:86a17116e8be | 476 | } |