├── .dockerignore ├── .github └── workflows │ └── build.yml ├── .gitignore ├── BNO055ESP32.cpp ├── CMakeLists.txt ├── Dockerfile.test_build ├── LICENSE ├── README.md ├── examples ├── example.cpp ├── exampleCalib.cpp ├── exampleInterrupts.cpp └── exampleQuaternion.cpp ├── idf_component.yml └── include └── BNO055ESP32.h /.dockerignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | .devcontainer 3 | Dockerfile.test_build -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: CI - Build 2 | 3 | on: 4 | push 5 | 6 | jobs: 7 | build: 8 | runs-on: ubuntu-latest 9 | timeout-minutes: 10 10 | steps: 11 | - uses: actions/checkout@v2 12 | 13 | - name: Install deps 14 | run: | 15 | sudo apt install -y git wget flex bison gperf python3 python3-pip python3-setuptools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0 16 | 17 | - name: Prepare test_app 18 | run: | 19 | cd ~ 20 | mkdir -p test_app/components/BNO055ESP32 21 | mkdir -p test_app/main/ 22 | echo -e "cmake_minimum_required(VERSION 3.5)\ninclude(\$ENV{IDF_PATH}/tools/cmake/project.cmake)\nproject(template-app)" > test_app/CMakeLists.txt 23 | echo -e "idf_component_register(SRCS "main.cpp" INCLUDE_DIRS ".")" > test_app/main/CMakeLists.txt 24 | cp -r $GITHUB_WORKSPACE test_app/components/ 25 | cp test_app/components/BNO055ESP32/examples/example.cpp test_app/main/main.cpp 26 | 27 | - name: Install esp-idf 28 | run: | 29 | mkdir -p ~/esp 30 | cd ~/esp 31 | git clone -b v5.1.2 --recursive https://github.com/espressif/esp-idf.git 32 | cd esp-idf 33 | ./install.sh esp32 34 | 35 | - name: Build test_app 36 | run: | 37 | . ~/esp/esp-idf/export.sh 38 | cd ~/test_app/ 39 | idf.py reconfigure 40 | echo -e "CONFIG_COMPILER_CXX_EXCEPTIONS=y\nCONFIG_COMPILER_CXX_EXCEPTIONS_EMG_POOL_SIZE=0\nCONFIG_CXX_EXCEPTIONS=y\nCONFIG_CXX_EXCEPTIONS_EMG_POOL_SIZE=0\n" >> ./sdkconfig 41 | idf.py build -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode -------------------------------------------------------------------------------- /BNO055ESP32.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2019 ShellAddicted 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | /* 24 | https://www.bosch-sensortec.com/bst/products/all_products/bno055 25 | Reference Datasheet: BST_BNO055_DS000_14 (consulted in January 2018) 26 | */ 27 | 28 | /*!please use the following clang-settings {BasedOnStyle: Google, ColumnLimit: 130, IndentWidth: 4}!*/ 29 | #include "BNO055ESP32.h" 30 | 31 | /* used in ESP_LOG macros */ 32 | static const char *BNO055_LOG_TAG = "BNO055"; 33 | 34 | BNO055::BNO055(uart_port_t uartPort, gpio_num_t txPin, gpio_num_t rxPin, gpio_num_t rstPin, gpio_num_t intPin) { 35 | _i2cFlag = false; 36 | 37 | _uartPort = uartPort; 38 | _txPin = txPin; 39 | _rxPin = rxPin; 40 | 41 | _rstPin = rstPin; 42 | _intPin = intPin; 43 | } 44 | 45 | BNO055::BNO055(i2c_port_t i2cPort, uint8_t i2cAddr, gpio_num_t rstPin, gpio_num_t intPin) { 46 | _i2cFlag = true; 47 | 48 | _i2cPort = i2cPort; 49 | _i2cAddr = i2cAddr; 50 | 51 | _rstPin = rstPin; 52 | _intPin = intPin; 53 | } 54 | 55 | BNO055::~BNO055() { stop(); } 56 | 57 | std::exception BNO055::getException(uint8_t errcode) { 58 | switch (errcode) { 59 | case 0x02: 60 | return BNO055ReadFail(); 61 | case 0x03: 62 | return BNO055WriteFail(); 63 | case 0x04: 64 | return BNO055RegmapInvalidAddress(); 65 | case 0x05: 66 | return BNO055RegmapWriteDisabled(); 67 | case 0x06: 68 | return BNO055WrongStartByte(); 69 | case 0x07: 70 | return BNO055BusOverRunError(); 71 | case 0x08: 72 | return BNO055MaxLengthError(); 73 | case 0x09: 74 | return BNO055MinLengthError(); 75 | case 0x0A: 76 | return BNO055ReceiveCharacterTimeout(); 77 | default: 78 | return BNO055UnknowError(); 79 | } 80 | } 81 | 82 | void BNO055::i2c_readLen(uint8_t reg, uint8_t *buffer, uint8_t len, uint32_t timeoutMS) { 83 | esp_err_t err = ESP_FAIL; 84 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 85 | i2c_master_start(cmd); 86 | i2c_master_write_byte(cmd, (_i2cAddr << 1) | I2C_MASTER_WRITE, ACK_EN); 87 | i2c_master_write_byte(cmd, reg, ACK_EN); 88 | i2c_master_stop(cmd); 89 | 90 | for (int round = 1; round <= UART_ROUND_NUM; round++) { 91 | #ifndef BNO055_DEBUG_OFF 92 | ESP_LOGD(BNO055_LOG_TAG, "(i2c_RL1) Round %d", round); 93 | #endif 94 | err = i2c_master_cmd_begin(_i2cPort, cmd, timeoutMS / portTICK_PERIOD_MS); 95 | if (err == ESP_OK) 96 | break; 97 | else 98 | ESP_LOGE(BNO055_LOG_TAG, "(i2c RL1) Error: %d", (int)err); 99 | } 100 | i2c_cmd_link_delete(cmd); 101 | if (err != ESP_OK) throw BNO055I2CError(); 102 | 103 | cmd = i2c_cmd_link_create(); 104 | i2c_master_start(cmd); 105 | i2c_master_write_byte(cmd, (_i2cAddr << 1) | I2C_MASTER_READ, ACK_EN); 106 | i2c_master_read(cmd, buffer, len, (i2c_ack_type_t)0x02); 107 | i2c_master_stop(cmd); 108 | for (int round = 1; round <= UART_ROUND_NUM; round++) { 109 | #ifndef BNO055_DEBUG_OFF 110 | ESP_LOGD(BNO055_LOG_TAG, "(i2c_RL2) Round %d", round); 111 | #endif 112 | err = i2c_master_cmd_begin(_i2cPort, cmd, timeoutMS / portTICK_PERIOD_MS); 113 | if (err == ESP_OK) 114 | break; 115 | else 116 | ESP_LOGE(BNO055_LOG_TAG, "(i2c RL2) Error: %d", (int)err); 117 | } 118 | i2c_cmd_link_delete(cmd); 119 | if (err != ESP_OK) throw BNO055I2CError(); 120 | } 121 | 122 | void BNO055::i2c_writeLen(uint8_t reg, uint8_t *buffer, uint8_t len, uint32_t timeoutMS) { 123 | esp_err_t err = ESP_FAIL; 124 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 125 | i2c_master_start(cmd); 126 | i2c_master_write_byte(cmd, (_i2cAddr << 1) | I2C_MASTER_WRITE, ACK_EN); 127 | i2c_master_write_byte(cmd, reg, ACK_EN); 128 | i2c_master_write(cmd, buffer, len, 0x01); 129 | i2c_master_stop(cmd); 130 | 131 | for (int round = 1; round <= UART_ROUND_NUM; round++) { 132 | #ifndef BNO055_DEBUG_OFF 133 | ESP_LOGD(BNO055_LOG_TAG, "(i2c_WL) Round %d", round); 134 | #endif 135 | err = i2c_master_cmd_begin(_i2cPort, cmd, timeoutMS / portTICK_PERIOD_MS); 136 | if (err == ESP_OK) 137 | break; 138 | else 139 | ESP_LOGE(BNO055_LOG_TAG, "(i2c WL) Error: %d", (int)err); 140 | } 141 | i2c_cmd_link_delete(cmd); 142 | if (err != ESP_OK) throw BNO055I2CError(); 143 | } 144 | 145 | void BNO055::uart_readLen(bno055_reg_t reg, uint8_t *buffer, uint8_t len, uint32_t timeoutMS) { 146 | uint8_t res = 0; 147 | uint8_t *data = NULL; 148 | int rxBytes = 0; 149 | uint8_t cmd[4]; 150 | 151 | cmd[0] = 0xAA; // Start Byte 152 | cmd[1] = 0x01; // Read 153 | cmd[2] = reg; 154 | cmd[3] = len; // len in bytes 155 | 156 | for (int round = 1; round <= UART_ROUND_NUM; round++) { 157 | #ifndef BNO055_DEBUG_OFF 158 | ESP_LOGD(BNO055_LOG_TAG, "(RL) Round %d", round); 159 | #endif 160 | 161 | uart_flush(_uartPort); 162 | uart_write_bytes(_uartPort, (const char *)cmd, 4); 163 | 164 | #ifndef BNO055_DEBUG_OFF 165 | ESP_LOG_BUFFER_HEXDUMP(BNO055_LOG_TAG, (const char *)cmd, 4, ESP_LOG_DEBUG); 166 | #endif 167 | 168 | if (timeoutMS > 0) { // check response (if expected) 169 | if (data == NULL) { 170 | data = (uint8_t *)malloc(len + 2); 171 | if (data == NULL) throw std::bad_alloc(); // malloc failed 172 | } 173 | rxBytes = uart_read_bytes(_uartPort, data, (len + 2), timeoutMS / portTICK_PERIOD_MS); 174 | if (rxBytes > 0) { 175 | #ifndef BNO055_DEBUG_OFF 176 | ESP_LOGD(BNO055_LOG_TAG, "(RL) Read %d bytes", rxBytes); 177 | ESP_LOG_BUFFER_HEXDUMP(BNO055_LOG_TAG, data, rxBytes, ESP_LOG_DEBUG); 178 | #endif 179 | if (data[0] == 0xBB) { 180 | res = 0; 181 | memcpy(buffer, data + 2, len); 182 | break; 183 | } else if (data[0] == 0xEE) { 184 | res = data[1]; 185 | #ifndef BNO055_DEBUG_OFF 186 | ESP_LOGD(BNO055_LOG_TAG, "(RL) Error: %d", res); 187 | #endif 188 | if (res != 0x07 && res != 0x02 && res != 0x0A) { 189 | ESP_LOGE(BNO055_LOG_TAG, "(RL) Error: %d", res); 190 | break; 191 | } 192 | } else { 193 | res = 0xFF; 194 | ESP_LOGE(BNO055_LOG_TAG, "(RL) Error: (BNO55_UNKNOW_ERROR)"); 195 | break; 196 | } 197 | } else 198 | break; 199 | } else 200 | break; 201 | } 202 | free(data); 203 | if (rxBytes <= 0 && timeoutMS > 0) throw BNO055UartTimeout(); 204 | if (res != 0) throw getException(res); 205 | } 206 | 207 | void BNO055::uart_writeLen(bno055_reg_t reg, uint8_t *data2write, uint8_t len, uint32_t timeoutMS) { 208 | uint8_t res = 0; 209 | uint8_t data[2]; 210 | int rxBytes = 0; 211 | uint8_t *cmd = (uint8_t *)malloc(len + 4); 212 | if (cmd == NULL) throw std::bad_alloc(); 213 | 214 | cmd[0] = 0xAA; // Start Byte 215 | cmd[1] = 0x00; // Write 216 | cmd[2] = reg; 217 | cmd[3] = len; // len in bytes 218 | memcpy(cmd + 4, data2write, len); 219 | 220 | // Read data from the UART 221 | for (int round = 1; round <= UART_ROUND_NUM; round++) { 222 | #ifndef BNO055_DEBUG_OFF 223 | ESP_LOGD(BNO055_LOG_TAG, "(WL) Round %d", round); // DEBUG 224 | #endif 225 | 226 | uart_flush(_uartPort); 227 | uart_write_bytes(_uartPort, (const char *)cmd, (len + 4)); 228 | 229 | #ifndef BNO055_DEBUG_OFF 230 | ESP_LOG_BUFFER_HEXDUMP(BNO055_LOG_TAG, (const char *)cmd, (len + 4), ESP_LOG_DEBUG); 231 | #endif 232 | 233 | if (timeoutMS > 0) { // check response (if expected) 234 | rxBytes = uart_read_bytes(_uartPort, data, 2, timeoutMS / portTICK_PERIOD_MS); 235 | if (rxBytes > 0) { 236 | #ifndef BNO055_DEBUG_OFF 237 | ESP_LOGD(BNO055_LOG_TAG, "(WL) Read %d bytes", rxBytes); // DEBUG 238 | ESP_LOG_BUFFER_HEXDUMP(BNO055_LOG_TAG, (const char *)data, rxBytes, ESP_LOG_DEBUG); 239 | #endif 240 | if (data[0] == 0xEE) { 241 | res = data[1]; 242 | if (res == 0x01) { 243 | res = 0; // Suppress exception 244 | break; 245 | } else if (res != 0x07 && res != 0x03 && res != 0x06 && res && 0x0A) 246 | break; 247 | } else { 248 | res = 0xFF; 249 | break; 250 | } 251 | } else 252 | break; 253 | } else 254 | break; 255 | } 256 | free(cmd); 257 | if (rxBytes <= 0 && timeoutMS > 0) throw BNO055UartTimeout(); 258 | if (res != 0) throw getException(res); 259 | } 260 | 261 | void BNO055::readLen(bno055_reg_t reg, uint8_t *buffer, uint8_t len, uint8_t page, uint32_t timeoutMS) { 262 | if (reg != BNO055_REG_PAGE_ID) setPage(page); 263 | if (_i2cFlag) { 264 | i2c_readLen(reg, buffer, len, timeoutMS); 265 | } else { 266 | uart_readLen(reg, buffer, len, timeoutMS); 267 | } 268 | } 269 | 270 | void BNO055::writeLen(bno055_reg_t reg, uint8_t *buffer, uint8_t len, uint8_t page, uint32_t timeoutMS) { 271 | if (reg != BNO055_REG_PAGE_ID) setPage(page); 272 | if (!_i2cFlag) { 273 | uart_writeLen(reg, buffer, len, timeoutMS); 274 | } else { 275 | i2c_writeLen(reg, buffer, len, timeoutMS); 276 | } 277 | } 278 | 279 | void BNO055::setPage(uint8_t page, bool forced) { 280 | if (_page != page || forced == true) { 281 | writeLen(BNO055_REG_PAGE_ID, &page); 282 | _page = page; 283 | } 284 | } 285 | 286 | void BNO055::setOprMode(bno055_opmode_t mode, bool forced) { 287 | if (_mode != mode || forced == true) { 288 | writeLen(BNO055_REG_OPR_MODE, (uint8_t *)&mode); 289 | vTaskDelay(30 / portTICK_PERIOD_MS); 290 | _mode = mode; 291 | } 292 | } 293 | 294 | void BNO055::setOprModeConfig(bool forced) { setOprMode(BNO055_OPERATION_MODE_CONFIG, forced); } 295 | 296 | void BNO055::setOprModeAccOnly(bool forced) { setOprMode(BNO055_OPERATION_MODE_ACCONLY, forced); } 297 | 298 | void BNO055::setOprModeMagOnly(bool forced) { setOprMode(BNO055_OPERATION_MODE_MAGONLY, forced); } 299 | 300 | void BNO055::setOprModeGyroOnly(bool forced) { setOprMode(BNO055_OPERATION_MODE_GYRONLY, forced); } 301 | 302 | void BNO055::setOprModeAccMag(bool forced) { setOprMode(BNO055_OPERATION_MODE_ACCMAG, forced); } 303 | 304 | void BNO055::setOprModeAccGyro(bool forced) { setOprMode(BNO055_OPERATION_MODE_ACCGYRO, forced); } 305 | 306 | void BNO055::setOprModeMagGyro(bool forced) { setOprMode(BNO055_OPERATION_MODE_MAGGYRO, forced); } 307 | 308 | void BNO055::setOprModeAMG(bool forced) { setOprMode(BNO055_OPERATION_MODE_AMG, forced); } 309 | 310 | void BNO055::setOprModeIMU(bool forced) { setOprMode(BNO055_OPERATION_MODE_IMU, forced); } 311 | 312 | void BNO055::setOprModeCompass(bool forced) { setOprMode(BNO055_OPERATION_MODE_COMPASS, forced); } 313 | 314 | void BNO055::setOprModeM4G(bool forced) { setOprMode(BNO055_OPERATION_MODE_M4G, forced); } 315 | 316 | void BNO055::setOprModeNdofFmcOff(bool forced) { setOprMode(BNO055_OPERATION_MODE_NDOF_FMC_OFF, forced); } 317 | 318 | void BNO055::setOprModeNdof(bool forced) { setOprMode(BNO055_OPERATION_MODE_NDOF, forced); } 319 | 320 | void BNO055::setPwrMode(bno055_powermode_t pwrMode) { 321 | if (_mode != BNO055_OPERATION_MODE_CONFIG) throw BNO055WrongOprMode("setPwrMode requires BNO055_OPERATION_MODE_CONFIG"); 322 | 323 | writeLen(BNO055_REG_PWR_MODE, (uint8_t *)&pwrMode); 324 | } 325 | 326 | void BNO055::setPwrModeNormal() { setPwrMode(BNO055_PWR_MODE_NORMAL); } 327 | void BNO055::setPwrModeLowPower() { setPwrMode(BNO055_PWR_MODE_LOWPOWER); } 328 | void BNO055::setPwrModeSuspend() { setPwrMode(BNO055_PWR_MODE_SUSPEND); } 329 | 330 | void BNO055::setExtCrystalUse(bool state) { 331 | uint8_t tmp = 0; 332 | if (_mode != BNO055_OPERATION_MODE_CONFIG) throw BNO055WrongOprMode("setExtCrystalUse requires BNO055_OPERATION_MODE_CONFIG"); 333 | 334 | readLen(BNO055_REG_SYS_TRIGGER, &tmp); 335 | tmp |= (state == true) ? 0x80 : 0x0; 336 | writeLen(BNO055_REG_SYS_TRIGGER, &tmp); 337 | 338 | vTaskDelay(650 / portTICK_PERIOD_MS); 339 | } 340 | 341 | void BNO055::enableExternalCrystal() { setExtCrystalUse(true); } 342 | 343 | void BNO055::disableExternalCrystal() { setExtCrystalUse(false); } 344 | 345 | int16_t BNO055::getSWRevision() { 346 | uint8_t buffer[2]; 347 | readLen(BNO055_REG_SW_REV_ID_LSB, buffer, 2); 348 | return (int16_t)((buffer[1] << 8) | buffer[0]); 349 | } 350 | 351 | uint8_t BNO055::getBootloaderRevision() { 352 | uint8_t tmp; 353 | readLen(BNO055_REG_BL_REV_ID, &tmp); 354 | return tmp; 355 | } 356 | 357 | bno055_system_status_t BNO055::getSystemStatus() { 358 | uint8_t tmp; 359 | readLen(BNO055_REG_SYS_STATUS, &tmp); 360 | return (bno055_system_status_t)tmp; 361 | } 362 | 363 | bno055_self_test_result_t BNO055::getSelfTestResult() { 364 | uint8_t tmp; 365 | bno055_self_test_result_t res; 366 | readLen(BNO055_REG_ST_RESULT, &tmp); 367 | res.mcuState = (tmp >> 3) & 0x01; 368 | res.gyrState = (tmp >> 2) & 0x01; 369 | res.magState = (tmp >> 1) & 0x01; 370 | res.accState = tmp & 0x01; 371 | return res; 372 | } 373 | 374 | bno055_system_error_t BNO055::getSystemError() { 375 | uint8_t tmp; 376 | readLen(BNO055_REG_SYS_ERR, &tmp); 377 | return (bno055_system_error_t)tmp; 378 | } 379 | 380 | bno055_calibration_t BNO055::getCalibration() { 381 | bno055_calibration_t cal; 382 | uint8_t calData; 383 | readLen(BNO055_REG_CALIB_STAT, &calData); 384 | cal.sys = (calData >> 6) & 0x03; 385 | cal.gyro = (calData >> 4) & 0x03; 386 | cal.accel = (calData >> 2) & 0x03; 387 | cal.mag = calData & 0x03; 388 | return cal; 389 | } 390 | 391 | int8_t BNO055::getTemp() { 392 | uint8_t t; 393 | readLen(BNO055_REG_TEMP, &t); 394 | t *= tempScale; 395 | return t; 396 | } 397 | 398 | void BNO055::reset() { 399 | int tmp = 0x20; 400 | if (_rstPin == GPIO_NUM_MAX) { 401 | #ifndef BNO055_DEBUG_OFF 402 | ESP_LOGD(BNO055_LOG_TAG, "RST -> using serial bus"); // DEBUG 403 | #endif 404 | writeLen(BNO055_REG_SYS_TRIGGER, (uint8_t *)&tmp, 1, 0, 0); // RST (0 timeout because RST is not Acknowledged) 405 | } else { 406 | #ifndef BNO055_DEBUG_OFF 407 | ESP_LOGD(BNO055_LOG_TAG, "RST -> using hardware pin"); // DEBUG 408 | #endif 409 | gpio_reset_pin(_rstPin); 410 | gpio_set_direction(_rstPin, GPIO_MODE_OUTPUT); 411 | gpio_set_level(_rstPin, 0); // turn OFF 412 | vTaskDelay(1 / portTICK_PERIOD_MS); 413 | gpio_set_level(_rstPin, 1); // turn ON 414 | } 415 | vTaskDelay(700 / portTICK_PERIOD_MS); // (RE)BOOT TIME (datasheet recommends 650ms) 416 | } 417 | 418 | bno055_vector_t BNO055::getVector(bno055_vector_type_t vec) { 419 | uint8_t buffer[6]; 420 | bno055_vector_t xyz; 421 | 422 | /* Read (6 bytes) */ 423 | readLen((bno055_reg_t)vec, buffer, 6); 424 | 425 | double scale = 1; 426 | 427 | switch (vec) { 428 | case BNO055_VECTOR_MAGNETOMETER: 429 | scale = magScale; 430 | break; 431 | case BNO055_VECTOR_ACCELEROMETER: 432 | case BNO055_VECTOR_LINEARACCEL: 433 | case BNO055_VECTOR_GRAVITY: 434 | scale = accelScale; 435 | break; 436 | case BNO055_VECTOR_GYROSCOPE: 437 | scale = angularRateScale; 438 | break; 439 | case BNO055_VECTOR_EULER: 440 | scale = eulerScale; 441 | break; 442 | default: 443 | break; 444 | } 445 | 446 | xyz.x = (int16_t)((buffer[1] << 8) | buffer[0]) / scale; 447 | xyz.y = (int16_t)((buffer[3] << 8) | buffer[2]) / scale; 448 | xyz.z = (int16_t)((buffer[5] << 8) | buffer[4]) / scale; 449 | 450 | return xyz; 451 | } 452 | 453 | bno055_vector_t BNO055::getVectorAccelerometer() { return getVector(BNO055_VECTOR_ACCELEROMETER); } 454 | 455 | bno055_vector_t BNO055::getVectorMagnetometer() { return getVector(BNO055_VECTOR_MAGNETOMETER); } 456 | 457 | bno055_vector_t BNO055::getVectorGyroscope() { return getVector(BNO055_VECTOR_GYROSCOPE); } 458 | 459 | bno055_vector_t BNO055::getVectorEuler() { return getVector(BNO055_VECTOR_EULER); } 460 | 461 | bno055_vector_t BNO055::getVectorLinearAccel() { return getVector(BNO055_VECTOR_LINEARACCEL); } 462 | 463 | bno055_vector_t BNO055::getVectorGravity() { return getVector(BNO055_VECTOR_GRAVITY); } 464 | 465 | bno055_quaternion_t BNO055::getQuaternion() { 466 | uint8_t buffer[8]; 467 | bno055_quaternion_t wxyz; 468 | 469 | double scale = 1 << 14; 470 | /* Read quat data (8 bytes) */ 471 | readLen(BNO055_REG_QUA_DATA_W_LSB, buffer, 8); 472 | 473 | wxyz.w = (int16_t)((buffer[1] << 8) | buffer[0]) / scale; 474 | wxyz.x = (int16_t)((buffer[3] << 8) | buffer[2]) / scale; 475 | wxyz.y = (int16_t)((buffer[5] << 8) | buffer[4]) / scale; 476 | wxyz.z = (int16_t)((buffer[7] << 8) | buffer[6]) / scale; 477 | 478 | return wxyz; 479 | } 480 | 481 | bno055_offsets_t BNO055::getSensorOffsets() { 482 | uint8_t buffer[22]; 483 | if (_mode != BNO055_OPERATION_MODE_CONFIG) throw BNO055WrongOprMode("getSensorOffsets requires BNO055_OPERATION_MODE_CONFIG"); 484 | 485 | /* Accel offset range depends on the G-range: 486 | +/-2g = +/- 2000 mg 487 | +/-4g = +/- 4000 mg 488 | +/-8g = +/- 8000 mg 489 | +/-1g = +/- 16000 mg 490 | */ 491 | bno055_offsets_t sensorOffsets; 492 | readLen(BNO055_REG_ACC_OFFSET_X_LSB, buffer, 22); 493 | 494 | sensorOffsets.accelOffsetX = ((buffer[1] << 8) | buffer[0]); 495 | sensorOffsets.accelOffsetY = ((buffer[3] << 8) | buffer[2]); 496 | sensorOffsets.accelOffsetZ = ((buffer[5] << 8) | buffer[4]); 497 | 498 | /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */ 499 | sensorOffsets.magOffsetX = ((buffer[7] << 8) | buffer[6]); 500 | sensorOffsets.magOffsetY = ((buffer[9] << 8) | buffer[8]); 501 | sensorOffsets.magOffsetZ = ((buffer[11] << 8) | buffer[10]); 502 | 503 | /* Gyro offset range depends on the DPS range: 504 | 2000 dps = +/- 32000 LSB 505 | 1000 dps = +/- 16000 LSB 506 | 500 dps = +/- 8000 LSB 507 | 250 dps = +/- 4000 LSB 508 | 125 dps = +/- 2000 LSB 509 | ... where 1 DPS = 16 LSB 510 | */ 511 | sensorOffsets.gyroOffsetX = ((buffer[13] << 8) | buffer[12]); 512 | sensorOffsets.gyroOffsetY = ((buffer[15] << 8) | buffer[14]); 513 | sensorOffsets.gyroOffsetZ = ((buffer[17] << 8) | buffer[16]); 514 | 515 | /* Accelerometer radius = +/- 1000 LSB */ 516 | sensorOffsets.accelRadius = ((buffer[19] << 8) | buffer[18]); 517 | 518 | /* Magnetometer radius = +/- 960 LSB */ 519 | sensorOffsets.magRadius = ((buffer[21] << 8) | buffer[20]); 520 | 521 | return sensorOffsets; 522 | } 523 | 524 | void BNO055::setSensorOffsets(bno055_offsets_t newOffsets) { 525 | if (_mode != BNO055_OPERATION_MODE_CONFIG) throw BNO055WrongOprMode("setSensorOffsets requires BNO055_OPERATION_MODE_CONFIG"); 526 | 527 | writeLen(BNO055_REG_ACC_OFFSET_X_LSB, (uint8_t *)&newOffsets, sizeof(newOffsets)); 528 | } 529 | 530 | bno055_interrupts_status_t BNO055::getInterruptsStatus() { 531 | uint8_t tmp = 0; 532 | bno055_interrupts_status_t status; 533 | readLen(BNO055_REG_INT_STA, &tmp); 534 | status.gyroAnyMotion = (tmp >> 2) & 0x01; 535 | status.gyroHR = (tmp >> 3) & 0x01; 536 | status.accelHighG = (tmp >> 5) & 0x01; 537 | status.accelAnyMotion = (tmp >> 6) & 0x01; 538 | status.accelNoSlowMotion = (tmp >> 7) & 0x01; 539 | return status; 540 | } 541 | 542 | void BNO055::clearInterruptPin() { 543 | interruptFlag = false; 544 | uint8_t tmp = 0; 545 | readLen(BNO055_REG_SYS_TRIGGER, &tmp); 546 | tmp |= 0x40; 547 | writeLen(BNO055_REG_SYS_TRIGGER, &tmp); 548 | } 549 | 550 | void IRAM_ATTR BNO055::bno055_interrupt_handler(void *arg) { static_cast(arg)->interruptFlag = true; } 551 | 552 | void BNO055::enableInterrupt(uint8_t flag, bool useInterruptPin) { 553 | uint8_t tmp[2]; 554 | 555 | readLen(BNO055_REG_INT_MSK, tmp, 2, 1); 556 | tmp[0] |= flag; 557 | tmp[1] = (useInterruptPin == true) ? (tmp[1] | flag) : (tmp[1] & ~flag); 558 | writeLen(BNO055_REG_INT_MSK, tmp, 2, 1); // update 559 | } 560 | 561 | void BNO055::disableInterrupt(uint8_t flag) { 562 | uint8_t tmp = 0; 563 | 564 | readLen(BNO055_REG_INT_EN, &tmp, 1, 1); 565 | tmp &= ~flag; 566 | writeLen(BNO055_REG_INT_EN, &tmp, 1, 1); // update 567 | } 568 | 569 | void BNO055::enableAccelSlowMotionInterrupt(bool useInterruptPin) { enableInterrupt(0x80, useInterruptPin); } 570 | 571 | void BNO055::setAccelSlowMotionInterrupt(uint8_t threshold, uint8_t duration, bool xAxis, bool yAxis, bool zAxis) { 572 | uint8_t tmp[2]; 573 | if (_mode != BNO055_OPERATION_MODE_CONFIG) 574 | throw BNO055WrongOprMode("setAccelSlowMotionInterrupt requires BNO055_OPERATION_MODE_CONFIG"); 575 | 576 | tmp[0] = threshold; 577 | tmp[1] = ((duration << 1) | 0x00); 578 | writeLen(BNO055_REG_ACC_NM_THRES, tmp, 2, 1); 579 | 580 | readLen(BNO055_REG_ACC_INT_SETTINGS, tmp, 1, 1); // read the current value to avoid overwrite of other bits 581 | tmp[0] = (xAxis == true) ? (tmp[0] | 0x04) : (tmp[0] & ~0x04); 582 | tmp[0] = (yAxis == true) ? (tmp[0] | 0x08) : (tmp[0] & ~0x08); 583 | tmp[0] = (zAxis == true) ? (tmp[0] | 0x10) : (tmp[0] & ~0x10); 584 | writeLen(BNO055_REG_ACC_INT_SETTINGS, tmp, 1, 1); // update 585 | } 586 | 587 | void BNO055::disableAccelSlowMotionInterrupt() { disableInterrupt(0x80); } 588 | 589 | void BNO055::enableAccelNoMotionInterrupt(bool useInterruptPin) { enableAccelSlowMotionInterrupt(useInterruptPin); } 590 | 591 | void BNO055::setAccelNoMotionInterrupt(uint8_t threshold, uint8_t duration, bool xAxis, bool yAxis, bool zAxis) { 592 | uint8_t tmp[2]; 593 | if (_mode != BNO055_OPERATION_MODE_CONFIG) 594 | throw BNO055WrongOprMode("setAccelNoMotionInterrupt requires BNO055_OPERATION_MODE_CONFIG"); 595 | 596 | tmp[0] = threshold; 597 | tmp[1] = ((duration << 1) | 0x01); 598 | writeLen(BNO055_REG_ACC_NM_THRES, tmp, 2, 1); 599 | 600 | readLen(BNO055_REG_ACC_INT_SETTINGS, tmp, 1, 1); 601 | tmp[0] = (xAxis == true) ? (tmp[0] | 0x04) : (tmp[0] & ~0x04); 602 | tmp[0] = (yAxis == true) ? (tmp[0] | 0x08) : (tmp[0] & ~0x08); 603 | tmp[0] = (zAxis == true) ? (tmp[0] | 0x10) : (tmp[0] & ~0x10); 604 | writeLen(BNO055_REG_ACC_INT_SETTINGS, tmp, 1, 1); // update 605 | } 606 | 607 | void BNO055::disableAccelNoMotionInterrupt() { disableAccelSlowMotionInterrupt(); } 608 | 609 | void BNO055::enableAccelAnyMotionInterrupt(bool useInterruptPin) { enableInterrupt(0x40, useInterruptPin); } 610 | 611 | void BNO055::setAccelAnyMotionInterrupt(uint8_t threshold, uint8_t duration, bool xAxis, bool yAxis, bool zAxis) { 612 | uint8_t tmp[2]; 613 | if (_mode != BNO055_OPERATION_MODE_CONFIG) 614 | throw BNO055WrongOprMode("setAccelAnyMotionInterrupt requires BNO055_OPERATION_MODE_CONFIG"); 615 | 616 | tmp[0] = threshold; 617 | readLen(BNO055_REG_ACC_INT_SETTINGS, tmp + 1, 1, 1); 618 | tmp[1] |= (duration & 0x03); 619 | tmp[1] = (xAxis == true) ? (tmp[1] | 0x04) : (tmp[1] & ~0x04); 620 | tmp[1] = (yAxis == true) ? (tmp[1] | 0x08) : (tmp[1] & ~0x08); 621 | tmp[1] = (zAxis == true) ? (tmp[1] | 0x10) : (tmp[1] & ~0x10); 622 | writeLen(BNO055_REG_ACC_AM_THRES, tmp, 2, 1); 623 | } 624 | 625 | void BNO055::disableAccelAnyMotionInterrupt() { disableInterrupt(0x40); } 626 | 627 | void BNO055::enableAccelHighGInterrupt(bool useInterruptPin) { enableInterrupt(0x20, useInterruptPin); } 628 | 629 | void BNO055::setAccelHighGInterrupt(uint8_t threshold, uint8_t duration, bool xAxis, bool yAxis, bool zAxis) { 630 | uint8_t tmp[3]; 631 | if (_mode != BNO055_OPERATION_MODE_CONFIG) 632 | throw BNO055WrongOprMode("setAccelHighGInterrupt requires BNO055_OPERATION_MODE_CONFIG"); 633 | 634 | readLen(BNO055_REG_ACC_INT_SETTINGS, tmp, 1, 1); 635 | tmp[0] = (xAxis == true) ? (tmp[0] | 0x20) : (tmp[0] & ~0x20); 636 | tmp[0] = (yAxis == true) ? (tmp[0] | 0x40) : (tmp[0] & ~0x40); 637 | tmp[0] = (zAxis == true) ? (tmp[0] | 0x80) : (tmp[0] & ~0x80); 638 | tmp[1] = duration; 639 | tmp[2] = threshold; 640 | writeLen(BNO055_REG_ACC_INT_SETTINGS, tmp, 3, 1); 641 | } 642 | 643 | void BNO055::disableAccelHighGInterrupt() { disableInterrupt(0x20); } 644 | 645 | void BNO055::enableGyroAnyMotionInterrupt(bool useInterruptPin) { enableInterrupt(0x04, useInterruptPin); } 646 | 647 | void BNO055::setGyroAnyMotionInterrupt(uint8_t threshold, uint8_t slopeSamples, uint8_t awakeDuration, bool xAxis, bool yAxis, 648 | bool zAxis, bool filtered) { 649 | uint8_t tmp[2]; 650 | if (_mode != BNO055_OPERATION_MODE_CONFIG) 651 | throw BNO055WrongOprMode("setGyroAnyMotionInterrupt requires BNO055_OPERATION_MODE_CONFIG"); 652 | 653 | tmp[0] = threshold; 654 | tmp[1] = 0x00 | (awakeDuration & 0x03); 655 | tmp[1] = (tmp[1] << 2) | (threshold & 0x03); 656 | writeLen(BNO055_REG_GYR_AM_THRES, tmp, 2, 1); 657 | 658 | readLen(BNO055_REG_GYR_INT_SETTING, tmp, 1, 1); 659 | tmp[0] = (xAxis == true) ? (tmp[0] | 0x01) : (tmp[0] & ~0x01); 660 | tmp[0] = (yAxis == true) ? (tmp[0] | 0x02) : (tmp[0] & ~0x02); 661 | tmp[0] = (zAxis == true) ? (tmp[0] | 0x04) : (tmp[0] & ~0x04); 662 | tmp[0] = (filtered == true) ? (tmp[0] & ~0x40) : (tmp[0] | 0x40); 663 | writeLen(BNO055_REG_GYR_INT_SETTING, tmp, 1, 1); 664 | } 665 | 666 | void BNO055::disableGyroAnyMotionInterrupt() { disableInterrupt(0x04); } 667 | 668 | void BNO055::enableGyroHRInterrupt(bool useInterruptPin) { enableInterrupt(0x08, useInterruptPin); } 669 | 670 | void BNO055::setGyroHRInterrupt(uint8_t thresholdX, uint8_t durationX, uint8_t hysteresisX, uint8_t thresholdY, uint8_t durationY, 671 | uint8_t hysteresisY, uint8_t thresholdZ, uint8_t durationZ, uint8_t hysteresisZ, bool xAxis, 672 | bool yAxis, bool zAxis, bool filtered) { 673 | uint8_t tmp[7]; 674 | if (_mode != BNO055_OPERATION_MODE_CONFIG) 675 | throw BNO055WrongOprMode("setGyroHRInterrupt requires BNO055_OPERATION_MODE_CONFIG"); 676 | 677 | readLen(BNO055_REG_GYR_INT_SETTING, tmp, 1, 1); 678 | tmp[0] = (xAxis == true) ? (tmp[0] | 0x01) : (tmp[0] & ~0x01); 679 | tmp[0] = (yAxis == true) ? (tmp[0] | 0x02) : (tmp[0] & ~0x02); 680 | tmp[0] = (zAxis == true) ? (tmp[0] | 0x04) : (tmp[0] & ~0x04); 681 | tmp[0] = (filtered == true) ? (tmp[0] & ~0x40) : (tmp[0] | 0x40); 682 | 683 | tmp[1] = 0x00 | (hysteresisX & 0x03); 684 | tmp[1] = (tmp[1] << 4) | (thresholdX & 0xF); 685 | 686 | tmp[2] = durationX; 687 | 688 | tmp[3] = 0x00 | (hysteresisY & 0x03); 689 | tmp[3] = (tmp[3] << 4) | (thresholdY & 0xF); 690 | 691 | tmp[4] = durationY; 692 | 693 | tmp[5] |= 0x00 | (hysteresisZ & 0x03); 694 | tmp[5] = (tmp[5] << 4) | (thresholdZ & 0xF); 695 | 696 | tmp[6] = durationZ; 697 | writeLen(BNO055_REG_GYR_INT_SETTING, tmp, 7, 1); 698 | } 699 | 700 | void BNO055::disableGyroHRInterrupt() { disableInterrupt(0x08); } 701 | 702 | void BNO055::setAxisRemap(bno055_axis_config_t config, bno055_axis_sign_t sign) { 703 | uint8_t tmp[2]; 704 | if (_mode != BNO055_OPERATION_MODE_CONFIG) throw BNO055WrongOprMode("setAxisRemap requires BNO055_OPERATION_MODE_CONFIG"); 705 | 706 | tmp[0] = ((uint8_t)config & 0x1F); 707 | tmp[1] = ((uint8_t)sign & 0x07); 708 | writeLen(BNO055_REG_AXIS_MAP_CONFIG, tmp, 2); 709 | } 710 | 711 | void BNO055::setUnits(bno055_accel_unit_t accel, bno055_angular_rate_unit_t angularRate, bno055_euler_unit_t euler, 712 | bno055_temperature_unit_t temp, bno055_data_output_format_t format) { 713 | uint8_t tmp = 0; 714 | if (_mode != BNO055_OPERATION_MODE_CONFIG) throw BNO055WrongOprMode("setUnits requires BNO055_OPERATION_MODE_CONFIG"); 715 | 716 | tmp |= accel; 717 | accelScale = (accel != 0) ? 1 : 100; 718 | 719 | tmp |= angularRate; 720 | angularRateScale = (angularRate != 0) ? 900 : 16; 721 | 722 | tmp |= euler; 723 | eulerScale = (euler != 0) ? 900 : 16; 724 | 725 | tmp |= temp; 726 | tempScale = (temp != 0) ? 2 : 1; 727 | 728 | tmp |= format; 729 | writeLen(BNO055_REG_UNIT_SEL, &tmp); 730 | } 731 | 732 | void BNO055::setAccelConfig(bno055_accel_range_t range, bno055_accel_bandwidth_t bandwidth, bno055_accel_mode_t mode) { 733 | uint8_t tmp = 0; 734 | if (_mode != BNO055_OPERATION_MODE_CONFIG) throw BNO055WrongOprMode("setAccelConfig requires BNO055_OPERATION_MODE_CONFIG"); 735 | 736 | tmp |= range; 737 | tmp |= bandwidth; 738 | tmp |= mode; 739 | writeLen(BNO055_REG_ACC_CONFIG, &tmp, 1, 1); 740 | } 741 | 742 | void BNO055::setGyroConfig(bno055_gyro_range_t range, bno055_gyro_bandwidth_t bandwidth, bno055_gyro_mode_t mode) { 743 | uint8_t tmp[2] = {0}; 744 | if (_mode != BNO055_OPERATION_MODE_CONFIG) throw BNO055WrongOprMode("setGyroConfig requires BNO055_OPERATION_MODE_CONFIG"); 745 | 746 | tmp[0] |= range; 747 | tmp[0] |= bandwidth; 748 | tmp[1] |= mode; 749 | writeLen(BNO055_REG_GYR_CONFIG_0, tmp, 2, 1); 750 | } 751 | 752 | void BNO055::setMagConfig(bno055_mag_rate_t rate, bno055_mag_pwrmode_t pwrmode, bno055_mag_mode_t mode) { 753 | uint8_t tmp = 0; 754 | if (_mode != BNO055_OPERATION_MODE_CONFIG) throw BNO055WrongOprMode("setMagConfig requires BNO055_OPERATION_MODE_CONFIG"); 755 | 756 | tmp |= rate; 757 | tmp |= pwrmode; 758 | tmp |= mode; 759 | writeLen(BN0055_REG_MAG_CONFIG, &tmp, 1, 1); 760 | } 761 | 762 | void BNO055::setGyroSleepConfig(bno055_gyro_auto_sleep_duration_t autoSleepDuration, bno055_gyro_sleep_duration_t sleepDuration) { 763 | uint8_t tmp = 0; 764 | if (_mode != BNO055_OPERATION_MODE_CONFIG) 765 | throw BNO055WrongOprMode("setGyroSleepConfig requires BNO055_OPERATION_MODE_CONFIG"); 766 | 767 | tmp |= autoSleepDuration; 768 | tmp |= sleepDuration; 769 | writeLen(BNO055_REG_GYR_SLEEP_CONFIG, &tmp, 1, 1); 770 | } 771 | 772 | void BNO055::setAccelSleepConfig(bno055_accel_sleep_duration_t sleepDuration, bno055_accel_sleep_mode_t sleepMode) { 773 | uint8_t tmp = 0; 774 | if (_mode != BNO055_OPERATION_MODE_CONFIG) 775 | throw BNO055WrongOprMode("setAccelSleepConfig requires BNO055_OPERATION_MODE_CONFIG"); 776 | tmp |= sleepDuration; 777 | tmp |= sleepMode; 778 | writeLen(BNO055_REG_ACC_SLEEP_CONFIG, &tmp, 1, 1); 779 | } 780 | 781 | void BNO055::begin() { 782 | uint8_t id = 0; 783 | if (!_i2cFlag) { 784 | // Setup UART 785 | esp_err_t esperr = uart_driver_delete(_uartPort); 786 | uart_param_config(_uartPort, &uart_config); 787 | uart_set_pin(_uartPort, _txPin, _rxPin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); 788 | esperr = uart_driver_install(_uartPort, 128 * 2, 0, 0, NULL, 0); 789 | if (esperr != ESP_OK) throw BNO055UartInitFailed(); 790 | } 791 | 792 | if (_intPin != GPIO_NUM_MAX) { 793 | gpio_reset_pin(_intPin); 794 | gpio_set_direction(_intPin, GPIO_MODE_INPUT); 795 | gpio_set_intr_type(_intPin, GPIO_INTR_POSEDGE); 796 | gpio_set_pull_mode(_intPin, GPIO_PULLDOWN_ONLY); 797 | gpio_intr_enable(_intPin); 798 | gpio_install_isr_service(0); 799 | gpio_isr_handler_add(_intPin, bno055_interrupt_handler, (void *)this); 800 | } 801 | reset(); 802 | readLen(BNO055_REG_CHIP_ID, &id); 803 | if (id != 0xA0) throw BNO055ChipNotDetected(); // this is not the correct device, check your wiring 804 | 805 | setOprMode(BNO055_OPERATION_MODE_CONFIG, true); // this should be the default OPR_MODE 806 | } 807 | 808 | void BNO055::stop() { 809 | // Free allocated resources 810 | // set BNO055 in supension mode to reduce power consumption 811 | try { 812 | setOprModeConfig(); 813 | setPwrModeSuspend(); 814 | } catch (BNO055BaseException &exc) { 815 | } catch (std::exception &exc) { 816 | } 817 | 818 | if (!_i2cFlag) { 819 | // free UART 820 | uart_driver_delete(_uartPort); 821 | } 822 | 823 | #ifndef BNO055_DEBUG_OFF 824 | ESP_LOGD(BNO055_LOG_TAG, "Destroyed"); 825 | #endif 826 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | idf_component_register(SRCS "BNO055ESP32.cpp" INCLUDE_DIRS "include" REQUIRES driver) 2 | -------------------------------------------------------------------------------- /Dockerfile.test_build: -------------------------------------------------------------------------------- 1 | ARG IDF_VER=v5.1.2 2 | FROM espressif/idf:${IDF_VER} 3 | 4 | SHELL ["/bin/bash", "-c"] 5 | 6 | WORKDIR /test/ 7 | 8 | RUN mkdir -p test_app/components/BNO055ESP32 && \ 9 | mkdir -p test_app/main/ && \ 10 | echo -e "cmake_minimum_required(VERSION 3.5)\ninclude(\$ENV{IDF_PATH}/tools/cmake/project.cmake)\nproject(template-app)" > test_app/CMakeLists.txt && \ 11 | echo -e "idf_component_register(SRCS "main.cpp" INCLUDE_DIRS ".")" > test_app/main/CMakeLists.txt 12 | 13 | COPY . test_app/components/BNO055ESP32/ 14 | 15 | RUN cp test_app/components/BNO055ESP32/examples/example.cpp test_app/main/main.cpp 16 | WORKDIR /test/test_app/ 17 | RUN source $IDF_PATH/export.sh && \ 18 | $IDF_PATH/tools/idf.py reconfigure && \ 19 | echo -e "CONFIG_CXX_EXCEPTIONS=y\nCONFIG_COMPILER_CXX_EXCEPTIONS=y\nCONFIG_COMPILER_CXX_EXCEPTIONS_EMG_POOL_SIZE=0\nCONFIG_CXX_EXCEPTIONS=y\nCONFIG_CXX_EXCEPTIONS_EMG_POOL_SIZE=0\n" >> ./sdkconfig && \ 20 | idf.py build -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 ShellAddicted 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BNO055ESP32 2 | C++ Interface for the [Bosch-Sensortec's BNO055](https://www.bosch-sensortec.com/bst/products/all_products/bno055) 3 | compatible with [Espressif's ESP32 SoC](https://www.espressif.com/en/products/hardware/esp32/overview) running [esp-idf](https://github.com/espressif/esp-idf). 4 | 5 | # Compatibility 6 | Tested on ESP32D0WDQ6 (DevKitC) with [Adafruit's BNO055 Breakout Board](https://www.adafruit.com/product/4646) 7 | 8 | ## Supported Interfaces 9 | 10 | | Interface | Notes | 11 | |-----------|----------------------| 12 | | UART | Fully Supported | 13 | | I²C | Partially Supported* | 14 | 15 | *unstable (due to clock stretching) 16 | 17 | # Getting Started 18 | ***NOTE: this code is not (yet) Production Ready.*** 19 | 20 | You can use this as a [managed-component](https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-guides/tools/idf-component-manager.html) for your project by adding the following to your `idf_component.yml`: 21 | ```yaml 22 | BNO055ESP32: 23 | path: . 24 | git: https://github.com/ShellAddicted/BNO055ESP32.git 25 | ``` 26 | 27 | Alternatively, you can use this as a component for your project: 28 | ```bash 29 | mkdir components/ 30 | cd components/ 31 | git clone https://github.com/ShellAddicted/BNO055ESP32.git 32 | ``` 33 | Remember to enable ```Compiler Options -> Enable C++ Exceptions``` using ```idf.py menuconfig```. 34 | 35 | For more details see [examples/](https://github.com/ShellAddicted/BNO055ESP32/tree/master/examples) 36 | 37 | 38 | # Wiring 39 | 40 | | IMU Pin | UART | I²C | 41 | |---------|--------------------------------|----------------------------| 42 | | PS1 | 3.3v | GND | 43 | | SCL | UART RX (Default: GPIO_NUM_17) | SCL (Default: GPIO_NUM_22) | 44 | | SDA | UART TX (Default: GPIO_NUM_16) | SDA (Default: GPIO_NUM_21) | 45 | -------------------------------------------------------------------------------- /examples/example.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2019 ShellAddicted 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | /* 24 | https://www.bosch-sensortec.com/bst/products/all_products/bno055 25 | Reference Datasheet: BST_BNO055_DS000_14 (consulted in January 2018) 26 | */ 27 | #include "BNO055ESP32.h" 28 | #include "freertos/FreeRTOS.h" 29 | #include "freertos/task.h" 30 | static const char* TAG = "BNO055ESP32Example"; 31 | 32 | extern "C" void app_main() { 33 | // see exampleCalibration.cpp for more details. 34 | // bno055_offsets_t storedOffsets; 35 | // storedOffsets.accelOffsetX = 29; 36 | // storedOffsets.accelOffsetY = 24; 37 | // storedOffsets.accelOffsetZ = 16; 38 | // storedOffsets.magOffsetX = -243; 39 | // storedOffsets.magOffsetY = -420; 40 | // storedOffsets.magOffsetZ = -131; 41 | // storedOffsets.gyroOffsetX = 1; 42 | // storedOffsets.gyroOffsetY = -1; 43 | // storedOffsets.gyroOffsetZ = 0; 44 | // storedOffsets.accelRadius = 0; 45 | // storedOffsets.magRadius = 662; 46 | 47 | /* to use I2C uncomment this block and remove line 66 48 | 49 | // Setup I²C 50 | i2c_config_t conf; 51 | conf.mode = I2C_MODE_MASTER; 52 | conf.sda_io_num = GPIO_NUM_21; 53 | conf.scl_io_num = GPIO_NUM_22; 54 | conf.sda_pullup_en = GPIO_PULLUP_ENABLE; 55 | conf.scl_pullup_en = GPIO_PULLUP_ENABLE; 56 | conf.master.clk_speed = 100000; 57 | conf.clk_flags = 0; 58 | i2c_param_config(I2C_NUM_0, &conf); 59 | i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0); 60 | i2c_set_timeout(I2C_NUM_0, 30000); 61 | 62 | //to use i²C leave the following line active 63 | BNO055 bno((i2c_port_t)I2C_NUM_0, 0x28); // BNO055 I2C Addr can be 0x28 or 0x29 (depends on your hardware) 64 | */ 65 | 66 | // to use UART use the following line active (UART is suggested) 67 | BNO055 bno(UART_NUM_1, GPIO_NUM_17, GPIO_NUM_16); 68 | 69 | try { 70 | bno.begin(); // BNO055 is in CONFIG_MODE until it is changed 71 | bno.enableExternalCrystal(); 72 | // bno.setSensorOffsets(storedOffsets); 73 | // bno.setAxisRemap(BNO055_REMAP_CONFIG_P1, BNO055_REMAP_SIGN_P1); // see datasheet, section 3.4 74 | /* you can specify a PoWeRMode using: 75 | - setPwrModeNormal(); (Default on startup) 76 | - setPwrModeLowPower(); 77 | - setPwrModeSuspend(); (while suspended bno055 must remain in CONFIG_MODE) 78 | */ 79 | bno.setOprModeNdof(); 80 | ESP_LOGI(TAG, "Setup Done."); 81 | } catch (BNO055BaseException& ex) { // see BNO055ESP32.h for more details about exceptions 82 | ESP_LOGE(TAG, "Setup Failed, Error: %s", ex.what()); 83 | return; 84 | } catch (std::exception& ex) { 85 | ESP_LOGE(TAG, "Setup Failed, Error: %s", ex.what()); 86 | return; 87 | } 88 | 89 | try { 90 | int8_t temperature = bno.getTemp(); 91 | ESP_LOGI(TAG, "TEMP: %d°C", temperature); 92 | 93 | int16_t sw = bno.getSWRevision(); 94 | uint8_t bl_rev = bno.getBootloaderRevision(); 95 | ESP_LOGI(TAG, "SW rev: %d, bootloader rev: %u", sw, bl_rev); 96 | 97 | bno055_self_test_result_t res = bno.getSelfTestResult(); 98 | ESP_LOGI(TAG, "Self-Test Results: MCU: %u, GYR:%u, MAG:%u, ACC: %u", res.mcuState, res.gyrState, res.magState, 99 | res.accState); 100 | } catch (BNO055BaseException& ex) { // see BNO055ESP32.h for more details about exceptions 101 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 102 | return; 103 | } catch (std::exception& ex) { 104 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 105 | return; 106 | } 107 | 108 | while (1) { 109 | try { 110 | // Calibration 3 = fully calibrated, 0 = not calibrated 111 | bno055_calibration_t cal = bno.getCalibration(); 112 | bno055_vector_t v = bno.getVectorEuler(); 113 | ESP_LOGI(TAG, "Euler: X: %.1f Y: %.1f Z: %.1f || Calibration SYS: %u GYRO: %u ACC:%u MAG:%u", v.x, v.y, v.z, cal.sys, 114 | cal.gyro, cal.accel, cal.mag); 115 | } catch (BNO055BaseException& ex) { 116 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 117 | return; 118 | } catch (std::exception& ex) { 119 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 120 | } 121 | vTaskDelay(100 / portTICK_PERIOD_MS); // in fusion mode max output rate is 100hz (actual rate: 100ms (10hz)) 122 | } 123 | /* to [forcefully] stop the communication, set BNO055 in PWR_MODE_SUSPEND, and free all the allocated resources you can use: 124 | 125 | bno.stop(); // (if you can use something it does NOT mean you should!!!!) 126 | 127 | in most cases (99.9%) you don't have to care about stop() just don't use it, 128 | use it only when NECESSARY otherwise destructor ~BNO055() will 'autonomously' take care of everything. 129 | 130 | DO NOT USE stop() to disable bno055 for short periods because it's inefficient, 131 | see setPwrMode() and setOprMode*() functions and datasheet to do that in the right way. 132 | */ 133 | } 134 | -------------------------------------------------------------------------------- /examples/exampleCalib.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2019 ShellAddicted 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | /* 24 | https://www.bosch-sensortec.com/bst/products/all_products/bno055 25 | Reference Datasheet: BST_BNO055_DS000_14 (consulted in January 2018) 26 | */ 27 | #include "BNO055ESP32.h" 28 | #include "freertos/FreeRTOS.h" 29 | #include "freertos/task.h" 30 | static const char* TAG = "BNO055ESP32Example"; 31 | 32 | extern "C" void app_main() { 33 | // see exampleCalibration.cpp for more details. 34 | // bno055_offsets_t storedOffsets; 35 | // storedOffsets.accelOffsetX = 29; 36 | // storedOffsets.accelOffsetY = 24; 37 | // storedOffsets.accelOffsetZ = 16; 38 | // storedOffsets.magOffsetX = -243; 39 | // storedOffsets.magOffsetY = -420; 40 | // storedOffsets.magOffsetZ = -131; 41 | // storedOffsets.gyroOffsetX = 1; 42 | // storedOffsets.gyroOffsetY = -1; 43 | // storedOffsets.gyroOffsetZ = 0; 44 | // storedOffsets.accelRadius = 0; 45 | // storedOffsets.magRadius = 662; 46 | 47 | /* to use I2C uncomment this block and remove line 66 48 | 49 | // Setup I²C 50 | i2c_config_t conf; 51 | conf.mode = I2C_MODE_MASTER; 52 | conf.sda_io_num = GPIO_NUM_21; 53 | conf.scl_io_num = GPIO_NUM_22; 54 | conf.sda_pullup_en = GPIO_PULLUP_ENABLE; 55 | conf.scl_pullup_en = GPIO_PULLUP_ENABLE; 56 | conf.master.clk_speed = 100000; 57 | conf.clk_flags = 0; 58 | i2c_param_config(I2C_NUM_0, &conf); 59 | i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0); 60 | i2c_set_timeout(I2C_NUM_0, 30000); 61 | 62 | //to use i²C leave the following line active 63 | BNO055 bno((i2c_port_t)I2C_NUM_0, 0x28); // BNO055 I2C Addr can be 0x28 or 0x29 (depends on your hardware) 64 | */ 65 | 66 | // to use UART use the following line active (UART is suggested) 67 | BNO055 bno(UART_NUM_1, GPIO_NUM_17, GPIO_NUM_16); 68 | 69 | try { 70 | bno.begin(); // BNO055 is in CONFIG_MODE until it is changed 71 | bno.enableExternalCrystal(); 72 | // bno.setSensorOffsets(storedOffsets); 73 | // bno.setAxisRemap(BNO055_REMAP_CONFIG_P1, BNO055_REMAP_SIGN_P1); // see datasheet, section 3.4 74 | /* you can specify a PoWeRMode using: 75 | - setPwrModeNormal(); (Default on startup) 76 | - setPwrModeLowPower(); 77 | - setPwrModeSuspend(); (while suspended bno055 must remain in CONFIG_MODE) 78 | */ 79 | bno.setOprModeNdof(); 80 | ESP_LOGI(TAG, "Setup Done."); 81 | } catch (BNO055BaseException& ex) { // see BNO055ESP32.h for more details about exceptions 82 | ESP_LOGE(TAG, "Setup Failed, Error: %s", ex.what()); 83 | return; 84 | } catch (std::exception& ex) { 85 | ESP_LOGE(TAG, "Setup Failed, Error: %s", ex.what()); 86 | return; 87 | } 88 | 89 | try { 90 | int8_t temperature = bno.getTemp(); 91 | ESP_LOGI(TAG, "TEMP: %d°C", temperature); 92 | 93 | int16_t sw = bno.getSWRevision(); 94 | uint8_t bl_rev = bno.getBootloaderRevision(); 95 | ESP_LOGI(TAG, "SW rev: %d, bootloader rev: %u", sw, bl_rev); 96 | 97 | bno055_self_test_result_t res = bno.getSelfTestResult(); 98 | ESP_LOGI(TAG, "Self-Test Results: MCU: %u, GYR:%u, MAG:%u, ACC: %u", res.mcuState, res.gyrState, res.magState, 99 | res.accState); 100 | } catch (BNO055BaseException& ex) { // see BNO055ESP32.h for more details about exceptions 101 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 102 | return; 103 | } catch (std::exception& ex) { 104 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 105 | return; 106 | } 107 | 108 | while (1) { 109 | try { 110 | // Calibration 3 = fully calibrated, 0 = not calibrated 111 | bno055_calibration_t cal = bno.getCalibration(); 112 | bno055_vector_t v = bno.getVectorEuler(); 113 | ESP_LOGI(TAG, "Euler: X: %.1f Y: %.1f Z: %.1f || Calibration SYS: %u GYRO: %u ACC:%u MAG:%u", v.x, v.y, v.z, cal.sys, 114 | cal.gyro, cal.accel, cal.mag); 115 | if (cal.gyro == 3 && cal.accel == 3 && cal.mag == 3) { 116 | ESP_LOGI(TAG, "Fully Calibrated."); 117 | bno.setOprModeConfig(); // Change to OPR_MODE 118 | bno055_offsets_t txt = bno.getSensorOffsets(); // NOTE: this must be executed in CONFIG_MODE 119 | ESP_LOGI(TAG, 120 | "\nOffsets:\nAccel: X:%d, Y:%d, Z:%d;\nMag: X:%d, Y:%d, Z:%d;\nGyro: X:%d, Y:%d, Z:%d;\nAccelRadius: " 121 | "%d;\nMagRadius: %d;\n", 122 | txt.accelOffsetX, txt.accelOffsetY, txt.accelOffsetZ, txt.magOffsetX, txt.magOffsetY, txt.magOffsetZ, 123 | txt.gyroOffsetX, txt.gyroOffsetY, txt.gyroOffsetZ, txt.accelRadius, txt.magRadius); 124 | ESP_LOGI(TAG, 125 | "Store this values, place them using setSensorOffsets() after every reset of the BNO055 to avoid the " 126 | "calibration process, unluckily MAG requires to be calibrated after every reset, for more information " 127 | "check datasheet."); 128 | break; 129 | } 130 | } catch (BNO055BaseException& ex) { 131 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 132 | return; 133 | } catch (std::exception& ex) { 134 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 135 | } 136 | vTaskDelay(100 / portTICK_PERIOD_MS); // in fusion mode max output rate is 100hz (actual rate: 100ms (10hz)) 137 | } 138 | /* to [forcefully] stop the communication, set BNO055 in PWR_MODE_SUSPEND, and free all the allocated resources you can use: 139 | 140 | bno.stop(); // (if you can use something it does NOT mean you should!!!!) 141 | 142 | in most cases (99.9%) you don't have to care about stop() just don't use it, 143 | use it only when NECESSARY otherwise destructor ~BNO055() will 'autonomously' take care of everything. 144 | 145 | DO NOT USE stop() to disable bno055 for short periods because it's inefficient, 146 | see setPwrMode() and setOprMode*() functions and datasheet to do that in the right way. 147 | */ 148 | } 149 | -------------------------------------------------------------------------------- /examples/exampleInterrupts.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2019 ShellAddicted 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | /* 24 | https://www.bosch-sensortec.com/bst/products/all_products/bno055 25 | Reference Datasheet: BST_BNO055_DS000_14 (consulted in January 2018) 26 | */ 27 | #include "BNO055ESP32.h" 28 | #include "freertos/FreeRTOS.h" 29 | #include "freertos/task.h" 30 | static const char* TAG = "BNO055ESP32Example"; 31 | 32 | extern "C" void app_main() { 33 | // see exampleCalibration.cpp for more details. 34 | // bno055_offsets_t storedOffsets; 35 | // storedOffsets.accelOffsetX = 29; 36 | // storedOffsets.accelOffsetY = 24; 37 | // storedOffsets.accelOffsetZ = 16; 38 | // storedOffsets.magOffsetX = -243; 39 | // storedOffsets.magOffsetY = -420; 40 | // storedOffsets.magOffsetZ = -131; 41 | // storedOffsets.gyroOffsetX = 1; 42 | // storedOffsets.gyroOffsetY = -1; 43 | // storedOffsets.gyroOffsetZ = 0; 44 | // storedOffsets.accelRadius = 0; 45 | // storedOffsets.magRadius = 662; 46 | 47 | /* to use I2C uncomment this block and remove line 66 48 | 49 | // Setup I²C 50 | i2c_config_t conf; 51 | conf.mode = I2C_MODE_MASTER; 52 | conf.sda_io_num = GPIO_NUM_21; 53 | conf.scl_io_num = GPIO_NUM_22; 54 | conf.sda_pullup_en = GPIO_PULLUP_ENABLE; 55 | conf.scl_pullup_en = GPIO_PULLUP_ENABLE; 56 | conf.master.clk_speed = 100000; 57 | conf.clk_flags = 0; 58 | i2c_param_config(I2C_NUM_0, &conf); 59 | i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0); 60 | i2c_set_timeout(I2C_NUM_0, 30000); 61 | 62 | //to use i²C leave the following line active 63 | BNO055 bno((i2c_port_t)I2C_NUM_0, 0x28); // BNO055 I2C Addr can be 0x28 or 0x29 (depends on your hardware) 64 | */ 65 | 66 | // to use UART use the following line active (UART is suggested) 67 | BNO055 bno(UART_NUM_1, GPIO_NUM_17, GPIO_NUM_16); 68 | 69 | try { 70 | bno.begin(); // BNO055 is in CONFIG_MODE until it is changed 71 | bno.enableExternalCrystal(); 72 | // bno.setSensorOffsets(storedOffsets); 73 | // bno.setAxisRemap(BNO055_REMAP_CONFIG_P1, BNO055_REMAP_SIGN_P1); // see datasheet, section 3.4 74 | /* you can specify a PoWeRMode using: 75 | - setPwrModeNormal(); (Default on startup) 76 | - setPwrModeLowPower(); 77 | - setPwrModeSuspend(); (while suspended bno055 must remain in CONFIG_MODE) 78 | */ 79 | bno.setAccelAnyMotionInterrupt(2, 2, true, true, true); // configure the interrupt, see datasheet for more details. 80 | bno.setAccelNoMotionInterrupt(0, 0, true, true, true); 81 | bno.enableAccelAnyMotionInterrupt(true); // you can disable it with disableAccelAnyMotionInterrupt(); 82 | bno.enableAccelNoMotionInterrupt(true); 83 | bno.setOprModeNdof(); 84 | ESP_LOGI(TAG, "Setup Done."); 85 | } catch (BNO055BaseException& ex) { // see BNO055ESP32.h for more details about exceptions 86 | ESP_LOGE(TAG, "Setup Failed, Error: %s", ex.what()); 87 | return; 88 | } catch (std::exception& ex) { 89 | ESP_LOGE(TAG, "Setup Failed, Error: %s", ex.what()); 90 | return; 91 | } 92 | 93 | try { 94 | int8_t temperature = bno.getTemp(); 95 | ESP_LOGI(TAG, "TEMP: %d°C", temperature); 96 | 97 | int16_t sw = bno.getSWRevision(); 98 | uint8_t bl_rev = bno.getBootloaderRevision(); 99 | ESP_LOGI(TAG, "SW rev: %d, bootloader rev: %u", sw, bl_rev); 100 | 101 | bno055_self_test_result_t res = bno.getSelfTestResult(); 102 | ESP_LOGI(TAG, "Self-Test Results: MCU: %u, GYR:%u, MAG:%u, ACC: %u", res.mcuState, res.gyrState, res.magState, 103 | res.accState); 104 | } catch (BNO055BaseException& ex) { // see BNO055ESP32.h for more details about exceptions 105 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 106 | return; 107 | } catch (std::exception& ex) { 108 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 109 | return; 110 | } 111 | 112 | while (1) { 113 | try { 114 | if (bno.interruptFlag == true) { 115 | // See bno055_interrupts_status_t for more details. 116 | bno055_interrupts_status_t ist = bno.getInterruptsStatus(); 117 | // remember that multiple interrupts can be triggered at the same time. so you shouldn't use 'else if' 118 | if (ist.accelAnyMotion == 1) { 119 | ESP_LOGI(TAG, "AccelAnyMotion Interrupt received."); 120 | } 121 | if (ist.accelNoSlowMotion == 1) { 122 | ESP_LOGI(TAG, "accelNoSlowMotion Interrupt received."); 123 | } 124 | bno.clearInterruptPin(); // don't forget to place this. 125 | } 126 | // Calibration 3 = fully calibrated, 0 = not calibrated 127 | bno055_calibration_t cal = bno.getCalibration(); 128 | bno055_vector_t v = bno.getVectorEuler(); 129 | ESP_LOGI(TAG, "Euler: X: %.1f Y: %.1f Z: %.1f || Calibration SYS: %u GYRO: %u ACC:%u MAG:%u", v.x, v.y, v.z, cal.sys, 130 | cal.gyro, cal.accel, cal.mag); 131 | } catch (BNO055BaseException& ex) { 132 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 133 | return; 134 | } catch (std::exception& ex) { 135 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 136 | } 137 | vTaskDelay(100 / portTICK_PERIOD_MS); // in fusion mode max output rate is 100hz (actual rate: 100ms (10hz)) 138 | } 139 | /* to [forcefully] stop the communication, set BNO055 in PWR_MODE_SUSPEND, and free all the allocated resources you can use: 140 | 141 | bno.stop(); // (if you can use something it does NOT mean you should!!!!) 142 | 143 | in most cases (99.9%) you don't have to care about stop() just don't use it, 144 | use it only when NECESSARY otherwise destructor ~BNO055() will 'autonomously' take care of everything. 145 | 146 | DO NOT USE stop() to disable bno055 for short periods because it's inefficient, 147 | see setPwrMode() and setOprMode*() functions and datasheet to do that in the right way. 148 | */ 149 | } 150 | -------------------------------------------------------------------------------- /examples/exampleQuaternion.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2019 ShellAddicted 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | /* 24 | https://www.bosch-sensortec.com/bst/products/all_products/bno055 25 | Reference Datasheet: BST_BNO055_DS000_14 (consulted in January 2018) 26 | */ 27 | #include "BNO055ESP32.h" 28 | #include "freertos/FreeRTOS.h" 29 | #include "freertos/task.h" 30 | static const char* TAG = "BNO055ESP32Example"; 31 | 32 | extern "C" void app_main() { 33 | // see exampleCalibration.cpp for more details. 34 | // bno055_offsets_t storedOffsets; 35 | // storedOffsets.accelOffsetX = 29; 36 | // storedOffsets.accelOffsetY = 24; 37 | // storedOffsets.accelOffsetZ = 16; 38 | // storedOffsets.magOffsetX = -243; 39 | // storedOffsets.magOffsetY = -420; 40 | // storedOffsets.magOffsetZ = -131; 41 | // storedOffsets.gyroOffsetX = 1; 42 | // storedOffsets.gyroOffsetY = -1; 43 | // storedOffsets.gyroOffsetZ = 0; 44 | // storedOffsets.accelRadius = 0; 45 | // storedOffsets.magRadius = 662; 46 | 47 | /* to use I2C uncomment this block and remove line 66 48 | 49 | // Setup I²C 50 | i2c_config_t conf; 51 | conf.mode = I2C_MODE_MASTER; 52 | conf.sda_io_num = GPIO_NUM_21; 53 | conf.scl_io_num = GPIO_NUM_22; 54 | conf.sda_pullup_en = GPIO_PULLUP_ENABLE; 55 | conf.scl_pullup_en = GPIO_PULLUP_ENABLE; 56 | conf.master.clk_speed = 100000; 57 | conf.clk_flags = 0; 58 | i2c_param_config(I2C_NUM_0, &conf); 59 | i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0); 60 | i2c_set_timeout(I2C_NUM_0, 30000); 61 | 62 | //to use i²C leave the following line active 63 | BNO055 bno((i2c_port_t)I2C_NUM_0, 0x28); // BNO055 I2C Addr can be 0x28 or 0x29 (depends on your hardware) 64 | */ 65 | 66 | // to use UART use the following line active (UART is suggested) 67 | BNO055 bno(UART_NUM_1, GPIO_NUM_17, GPIO_NUM_16); 68 | 69 | try { 70 | bno.begin(); // BNO055 is in CONFIG_MODE until it is changed 71 | bno.enableExternalCrystal(); 72 | // bno.setSensorOffsets(storedOffsets); 73 | // bno.setAxisRemap(BNO055_REMAP_CONFIG_P1, BNO055_REMAP_SIGN_P1); // see datasheet, section 3.4 74 | bno.setOprModeNdof(); 75 | // bno.setSensorOffsets(storedOffsets); 76 | // bno.setAxisRemap(BNO055_REMAP_CONFIG_P1, BNO055_REMAP_SIGN_P1); // see datasheet, section 3.4 77 | /* you can specify a PoWeRMode using: 78 | - setPwrModeNormal(); (Default on startup) 79 | - setPwrModeLowPower(); 80 | - setPwrModeSuspend(); (while suspended bno055 must remain in CONFIG_MODE) 81 | */ 82 | ESP_LOGI(TAG, "Setup Done."); 83 | } catch (BNO055BaseException& ex) { // see BNO055ESP32.h for more details about exceptions 84 | ESP_LOGE(TAG, "Setup Failed, Error: %s", ex.what()); 85 | return; 86 | } catch (std::exception& ex) { 87 | ESP_LOGE(TAG, "Setup Failed, Error: %s", ex.what()); 88 | return; 89 | } 90 | 91 | try { 92 | int8_t temperature = bno.getTemp(); 93 | ESP_LOGI(TAG, "TEMP: %d°C", temperature); 94 | 95 | int16_t sw = bno.getSWRevision(); 96 | uint8_t bl_rev = bno.getBootloaderRevision(); 97 | ESP_LOGI(TAG, "SW rev: %d, bootloader rev: %u", sw, bl_rev); 98 | 99 | bno055_self_test_result_t res = bno.getSelfTestResult(); 100 | ESP_LOGI(TAG, "Self-Test Results: MCU: %u, GYR:%u, MAG:%u, ACC: %u", res.mcuState, res.gyrState, res.magState, 101 | res.accState); 102 | } catch (BNO055BaseException& ex) { // see BNO055ESP32.h for more details about exceptions 103 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 104 | return; 105 | } catch (std::exception& ex) { 106 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 107 | return; 108 | } 109 | 110 | while (1) { 111 | try { 112 | // Calibration 3 = fully calibrated, 0 = not calibrated 113 | bno055_calibration_t cal = bno.getCalibration(); 114 | bno055_quaternion_t v = bno.getQuaternion(); 115 | ESP_LOGI(TAG, "Quaternion: W: %1.f X: %.1f Y: %.1f Z: %.1f || Calibration SYS: %u GYRO: %u ACC:%u MAG:%u", v.w, v.x, 116 | v.y, v.z, cal.sys, cal.gyro, cal.accel, cal.mag); 117 | } catch (BNO055BaseException& ex) { 118 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 119 | return; 120 | } catch (std::exception& ex) { 121 | ESP_LOGE(TAG, "Something bad happened: %s", ex.what()); 122 | } 123 | vTaskDelay(100 / portTICK_PERIOD_MS); // in fusion mode max output rate is 100hz (actual rate: 100ms (10hz)) 124 | } 125 | /* to [forcefully] stop the communication, set BNO055 in PWR_MODE_SUSPEND, and free all the allocated resources you can use: 126 | 127 | bno.stop(); // (if you can use something it does NOT mean you should!!!!) 128 | 129 | in most cases (99.9%) you don't have to care about stop() just don't use it, 130 | use it only when NECESSARY otherwise destructor ~BNO055() will 'autonomously' take care of everything. 131 | 132 | DO NOT USE stop() to disable bno055 for short periods because it's inefficient, 133 | see setPwrMode() and setOprMode*() functions and datasheet to do that in the right way. 134 | */ 135 | } 136 | -------------------------------------------------------------------------------- /idf_component.yml: -------------------------------------------------------------------------------- 1 | version: "1.3.0" 2 | description: A C++ Interface for Bosch-Sensortec's BNO055 IMU 3 | url: https://github.com/ShellAddicted/BNO055ESP32.git 4 | dependencies: 5 | idf: ">=5.1" -------------------------------------------------------------------------------- /include/BNO055ESP32.h: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2019 ShellAddicted 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | /* 24 | https://www.bosch-sensortec.com/bst/products/all_products/bno055 25 | Reference Datasheet: BST_BNO055_DS000_14 (consulted in January 2018) 26 | */ 27 | 28 | #ifndef _BNO055ESP32_H_ 29 | #define _BNO055ESP32_H_ 30 | 31 | #define BNO055_DEBUG_OFF // uncomment this to DISABLE DEBUG LOGS 32 | 33 | #include //memset, memcpy 34 | #include 35 | #include 36 | #include "driver/i2c.h" 37 | #include "driver/uart.h" 38 | #include "freertos/FreeRTOS.h" 39 | #include "freertos/task.h" 40 | 41 | #define ACK_EN 0x01 42 | 43 | #ifndef BNO055_DEBUG_OFF 44 | #define LOG_LOCAL_LEVEL ESP_LOG_VERBOSE 45 | #endif 46 | #include "esp_log.h" 47 | 48 | #define DEFAULT_UART_TIMEOUT_MS 30 // you can try to decrease/increase this. (DEFAULT: 30) 49 | 50 | // BNO055 Registers(Table 4-1, Pag 51) 51 | typedef enum { 52 | // PAGE 1 53 | BNO055_REG_ACC_CONFIG = 0x08, 54 | BN0055_REG_MAG_CONFIG = 0x09, 55 | BNO055_REG_GYR_CONFIG_0 = 0x0A, 56 | BNO055_REG_GYR_CONFIG_1 = 0x0B, 57 | 58 | BNO055_REG_ACC_SLEEP_CONFIG = 0x0C, 59 | BNO055_REG_GYR_SLEEP_CONFIG = 0x0D, 60 | 61 | BNO055_REG_INT_MSK = 0x0F, 62 | BNO055_REG_INT_EN = 0x10, 63 | 64 | BNO055_REG_ACC_AM_THRES = 0x11, 65 | BNO055_REG_ACC_INT_SETTINGS = 0x12, 66 | BNO055_REG_ACC_HG_DURATION = 0x13, 67 | BNO055_REG_ACC_HG_THRES = 0x14, 68 | BNO055_REG_ACC_NM_THRES = 0x15, 69 | BNO055_REG_ACC_NM_SET = 0x16, 70 | BNO055_REG_GYR_INT_SETTING = 0x17, 71 | BNO055_REG_GYR_HR_X_SET = 0x18, 72 | BNO055_REG_GYR_DUR_X = 0x19, 73 | BNO055_REG_GYR_HR_Y_SET = 0x1A, 74 | BNO055_REG_GYR_DUR_Y = 0x1B, 75 | BNO055_REG_GYR_HR_Z_SET = 0x1C, 76 | BNO055_REG_GYR_DUR_Z = 0x1D, 77 | BNO055_REG_GYR_AM_THRES = 0x1E, 78 | BNO055_REG_GYR_AM_SET = 0x1F, 79 | 80 | BNO055_REG_PAGE_ID = 0x07, 81 | 82 | // PAGE 0 83 | BNO055_REG_CHIP_ID = 0x00, 84 | BNO055_REG_ACC_ID = 0x01, 85 | BNO055_REG_MAG_ID = 0x02, 86 | BNO055_REG_GYRO_ID = 0x03, 87 | BNO055_REG_SW_REV_ID_LSB = 0x04, 88 | BNO055_REG_SW_REV_ID_MSB = 0x05, 89 | BNO055_REG_BL_REV_ID = 0x06, 90 | 91 | BNO055_REG_ACC_DATA_X_LSB = 0x08, 92 | BNO055_REG_ACC_DATA_X_MSB = 0x09, 93 | BNO055_REG_ACC_DATA_Y_LSB = 0x0A, 94 | BNO055_REG_ACC_DATA_Y_MSB = 0x0B, 95 | BNO055_REG_ACC_DATA_Z_LSB = 0x0C, 96 | BNO055_REG_ACC_DATA_Z_MSB = 0x0D, 97 | 98 | BNO055_REG_MAG_DATA_X_LSB = 0x0E, 99 | BNO055_REG_MAG_DATA_X_MSB = 0x0F, 100 | BNO055_REG_MAG_DATA_Y_LSB = 0x10, 101 | BNO055_REG_MAG_DATA_Y_MSB = 0x11, 102 | BNO055_REG_MAG_DATA_Z_LSB = 0x12, 103 | BNO055_REG_MAG_DATA_Z_MSB = 0x13, 104 | 105 | BNO055_REG_GYR_DATA_X_LSB = 0x14, 106 | BNO055_REG_GYR_DATA_X_MSB = 0x15, 107 | BNO055_REG_GYR_DATA_Y_LSB = 0x16, 108 | BNO055_REG_GYR_DATA_Y_MSB = 0x17, 109 | BNO055_REG_GYR_DATA_Z_LSB = 0x18, 110 | BNO055_REG_GYR_DATA_Z_MSB = 0x19, 111 | 112 | BNO055_REG_EUL_HEADING_LSB = 0x1A, 113 | BNO055_REG_EUL_HEADING_MSB = 0x1B, 114 | BNO055_REG_EUL_ROLL_LSB = 0x1C, 115 | BNO055_REG_EUL_ROLL_MSB = 0x1D, 116 | BNO055_REG_EUL_PITCH_LSB = 0x1E, 117 | BNO055_REG_EUL_PITCH_MSB = 0x1F, 118 | 119 | BNO055_REG_QUA_DATA_W_LSB = 0x20, 120 | BNO055_REG_QUA_DATA_W_MSB = 0x21, 121 | BNO055_REG_QUA_DATA_X_LSB = 0x22, 122 | BNO055_REG_QUA_DATA_X_MSB = 0x23, 123 | BNO055_REG_QUA_DATA_Y_LSB = 0x24, 124 | BNO055_REG_QUA_DATA_Y_MSB = 0x25, 125 | BNO055_REG_QUA_DATA_Z_LSB = 0x26, 126 | BNO055_REG_QUA_DATA_Z_MSB = 0x27, 127 | 128 | BNO055_REG_LIA_DATA_X_LSB = 0x28, 129 | BNO055_REG_LIA_DATA_X_MSB = 0x29, 130 | BNO055_REG_LIA_DATA_Y_LSB = 0x2A, 131 | BNO055_REG_LIA_DATA_Y_MSB = 0x2B, 132 | BNO055_REG_LIA_DATA_Z_LSB = 0x2C, 133 | BNO055_REG_LIA_DATA_Z_MSB = 0x2D, 134 | 135 | BNO055_REG_GRV_DATA_X_LSB = 0x2E, 136 | BNO055_REG_GRV_DATA_X_MSB = 0x2F, 137 | BNO055_REG_GRV_DATA_Y_LSB = 0x30, 138 | BNO055_REG_GRV_DATA_Y_MSB = 0x31, 139 | BNO055_REG_GRV_DATA_Z_LSB = 0x32, 140 | BNO055_REG_GRV_DATA_Z_MSB = 0x33, 141 | 142 | BNO055_REG_TEMP = 0x34, 143 | 144 | BNO055_REG_CALIB_STAT = 0x35, 145 | BNO055_REG_ST_RESULT = 0x36, 146 | BNO055_REG_INT_STA = 0x37, 147 | 148 | BNO055_REG_SYS_CLK_STAT = 0x38, 149 | BNO055_REG_SYS_STATUS = 0x39, 150 | BNO055_REG_SYS_ERR = 0x3A, 151 | 152 | BNO055_REG_UNIT_SEL = 0x3B, 153 | 154 | BNO055_REG_OPR_MODE = 0x3D, 155 | BNO055_REG_PWR_MODE = 0x3E, 156 | BNO055_REG_SYS_TRIGGER = 0x3F, 157 | BNO055_REG_TEMP_SOURCE = 0x40, 158 | 159 | BNO055_REG_AXIS_MAP_CONFIG = 0x41, 160 | BNO055_REG_AXIS_MAP_SIGN = 0x42, 161 | 162 | BNO055_REG_ACC_OFFSET_X_LSB = 0x55, 163 | BNO055_REG_ACC_OFFSET_X_MSB = 0x56, 164 | BNO055_REG_ACC_OFFSET_Y_LSB = 0x57, 165 | BNO055_REG_ACC_OFFSET_Y_MSB = 0x58, 166 | BNO055_REG_ACC_OFFSET_Z_LSB = 0x59, 167 | BNO055_REG_ACC_OFFSET_Z_MSB = 0x5A, 168 | 169 | BNO055_REG_MAG_OFFSET_X_LSB = 0x5B, 170 | BNO055_REG_MAG_OFFSET_X_MSB = 0x5C, 171 | BNO055_REG_MAG_OFFSET_Y_LSB = 0x5D, 172 | BNO055_REG_MAG_OFFSET_Y_MSB = 0x5E, 173 | BNO055_REG_MAG_OFFSET_Z_LSB = 0x5F, 174 | BNO055_REG_MAG_OFFSET_Z_MSB = 0x60, 175 | 176 | BNO055_REG_GYR_OFFSET_X_LSB = 0x61, 177 | BNO055_REG_GYR_OFFSET_X_MSB = 0x62, 178 | BNO055_REG_GYR_OFFSET_Y_LSB = 0x63, 179 | BNO055_REG_GYR_OFFSET_Y_MSB = 0x64, 180 | BNO055_REG_GYR_OFFSET_Z_LSB = 0x65, 181 | BNO055_REG_GYR_OFFSET_Z_MSB = 0x66, 182 | 183 | BNO055_REG_ACC_RADIUS_LSB = 0x67, 184 | BNO055_REG_ACC_RADIUS_MSB = 0x68, 185 | 186 | BNO055_REG_MAG_RADIUS_LSB = 0x69, 187 | BNO055_REG_MAG_RADIUS_MSB = 0x6A 188 | } bno055_reg_t; 189 | 190 | /* System Status [SYS_STATUS] (sec: 4.3.58) 191 | 0 = Idle 192 | 1 = System Error 193 | 2 = Initializing Peripherals 194 | 3 = System Iniitalization 195 | 4 = Executing Self-Test 196 | 5 = Sensor fusion algorithm running 197 | 6 = System running without fusion algorithms 198 | */ 199 | typedef enum { 200 | BNO055_SYSTEM_STATUS_IDLE = 0x00, 201 | BNO055_SYSTEM_STATUS_SYSTEM_ERROR = 0x01, 202 | BNO055_SYSTEM_STATUS_INITIALIZING_PERIPHERALS = 0x02, 203 | BNO055_SYSTEM_STATUS_SYSTEM_INITIALIZATION = 0x03, 204 | BNO055_SYSTEM_STATUS_EXECUTING_SELF_TEST = 0x04, 205 | BNO055_SYSTEM_STATUS_FUSION_ALGO_RUNNING = 0x05, 206 | BNO055_SYSTEM_STATUS_FUSION_ALOG_NOT_RUNNING = 0x06 207 | } bno055_system_status_t; 208 | 209 | /* System Error [SYS_ERR] (sec: 4.3.59) 210 | 0 = No error 211 | 1 = Peripheral initialization error 212 | 2 = System initialization error 213 | 3 = Self test result failed 214 | 4 = Register map value out of range 215 | 5 = Register map address out of range 216 | 6 = Register map write error 217 | 7 = BNO low power mode not available for selected operat ion mode 218 | 8 = Accelerometer power mode not available 219 | 9 = Fusion algorithm configuration error 220 | A = Sensor configuration error 221 | */ 222 | typedef enum { 223 | BNO055_SYSTEM_ERROR_NO_ERROR = 0x00, 224 | BNO055_SYSTEM_ERROR_PERIPHERAL_INITIALIZATION_ERROR = 0x01, 225 | BNO055_SYSTEM_ERROR_SYSTEM_INITIALIZATION_ERROR = 0x02, 226 | BNO055_SYSTEM_ERROR_SELF_TEST_FAILED = 0x03, 227 | BNO055_SYSTEM_ERROR_REG_MAP_VAL_OUT_OF_RANGE = 0x04, 228 | BNO055_SYSTEM_ERROR_REG_MAP_ADDR_OUT_OF_RANGE = 0x05, 229 | BNO055_SYSTEM_ERROR_REG_MAP_WRITE_ERROR = 0x06, 230 | BNO055_SYSTEM_ERROR_LOW_PWR_MODE_NOT_AVAILABLE_FOR_SELECTED_OPR_MODE = 0x07, 231 | BNO055_SYSTEM_ERROR_ACCEL_PWR_MODE_NOT_AVAILABLE = 0x08, 232 | BNO055_SYSTEM_ERROR_FUSION_ALGO_CONF_ERROR = 0x09, 233 | BNO055_SYSTEM_ERROR_SENSOR_CONF_ERROR = 0x0A 234 | } bno055_system_error_t; 235 | 236 | typedef enum { 237 | BNO055_PWR_MODE_NORMAL = 0x00, 238 | BNO055_PWR_MODE_LOWPOWER = 0x01, 239 | BNO055_PWR_MODE_SUSPEND = 0x02 240 | } bno055_powermode_t; 241 | 242 | typedef enum { 243 | BNO055_REMAP_CONFIG_P0 = 0x21, 244 | BNO055_REMAP_CONFIG_P1 = 0x24, 245 | BNO055_REMAP_CONFIG_P2 = 0x24, 246 | BNO055_REMAP_CONFIG_P3 = 0x21, 247 | BNO055_REMAP_CONFIG_P4 = 0x24, 248 | BNO055_REMAP_CONFIG_P5 = 0x21, 249 | BNO055_REMAP_CONFIG_P6 = 0x21, 250 | BNO055_REMAP_CONFIG_P7 = 0x24 251 | } bno055_axis_config_t; 252 | 253 | typedef enum { 254 | BNO055_REMAP_SIGN_P0 = 0x04, 255 | BNO055_REMAP_SIGN_P1 = 0x00, 256 | BNO055_REMAP_SIGN_P2 = 0x06, 257 | BNO055_REMAP_SIGN_P3 = 0x02, 258 | BNO055_REMAP_SIGN_P4 = 0x03, 259 | BNO055_REMAP_SIGN_P5 = 0x01, 260 | BNO055_REMAP_SIGN_P6 = 0x07, 261 | BNO055_REMAP_SIGN_P7 = 0x05 262 | } bno055_axis_sign_t; 263 | 264 | typedef struct { 265 | uint8_t mcuState = 0; 266 | uint8_t gyrState = 0; 267 | uint8_t magState = 0; 268 | uint8_t accState = 0; 269 | } bno055_self_test_result_t; 270 | 271 | typedef struct { 272 | double x = 0; 273 | double y = 0; 274 | double z = 0; 275 | } bno055_vector_t; 276 | 277 | typedef struct { 278 | double w = 0; 279 | double x = 0; 280 | double y = 0; 281 | double z = 0; 282 | } bno055_quaternion_t; 283 | 284 | typedef enum { 285 | BNO055_UNIT_ACCEL_MS2 = 0x00, // m/s² 286 | BNO055_UNIT_ACCEL_MG = 0X01 287 | } bno055_accel_unit_t; 288 | 289 | typedef enum { BNO055_UNIT_ANGULAR_RATE_DPS = 0x00, BNO055_UNIT_ANGULAR_RATE_RPS = 0x02 } bno055_angular_rate_unit_t; 290 | 291 | typedef enum { BNO055_UNIT_EULER_DEGREES = 0x00, BNO055_UNIT_EULER_RADIANS = 0x04 } bno055_euler_unit_t; 292 | 293 | typedef enum { BNO055_UNIT_TEMP_C = 0x00, BNO055_UNIT_TEMP_F = 0x10 } bno055_temperature_unit_t; 294 | 295 | typedef enum { BNO055_DATA_FORMAT_WINDOWS = 0x00, BNO055_DATA_FORMAT_ANDROID = 0x80 } bno055_data_output_format_t; 296 | 297 | typedef enum { 298 | BNO055_CONF_ACCEL_RANGE_2G = 0x00, 299 | BNO055_CONF_ACCEL_RANGE_4G = 0x01, 300 | BNO055_CONF_ACCEL_RANGE_8G = 0x02, 301 | BNO055_CONF_ACCEL_RANGE_16G = 0x03, 302 | } bno055_accel_range_t; 303 | 304 | typedef enum { 305 | BNO055_CONF_ACCEL_BANDWIDTH_7_81HZ = 0x00, 306 | BNO055_CONF_ACCEL_BANDWIDTH_15_63HZ = 0x04, 307 | BNO055_CONF_ACCEL_BANDWIDTH_31_25HZ = 0x08, 308 | BNO055_CONF_ACCEL_BANDWIDTH_62_5HZ = 0x0C, 309 | BNO055_CONF_ACCEL_BANDWIDTH_125HZ = 0x10, 310 | BNO055_CONF_ACCEL_BANDWIDTH_250HZ = 0x14, 311 | BNO055_CONF_ACCEL_BANDWIDTH_500HZ = 0x18, 312 | BNO055_CONF_ACCEL_BANDWIDTH_1000HZ = 0x1C 313 | } bno055_accel_bandwidth_t; 314 | 315 | typedef enum { 316 | BNO055_CONF_ACCEL_MODE_NORMAL = 0x00, 317 | BNO055_CONF_ACCEL_MODE_SUSPEND = 0x20, 318 | BNO055_CONF_ACCEL_MODE_LOW_POWER1 = 0x40, 319 | BNO055_CONF_ACCEL_MODE_STANDBY = 0x60, 320 | BNO055_CONF_ACCEL_MODE_LOW_POWER2 = 0x80, 321 | BNO055_CONF_ACCEL_MODE_DEEP_SUSPEND = 0xA0 322 | } bno055_accel_mode_t; 323 | 324 | typedef enum { 325 | BNO055_CONF_GYRO_RANGE_2000DPS = 0x00, 326 | BNO055_CONF_GYRO_RANGE_1000DPS = 0x01, 327 | BNO055_CONF_GYRO_RANGE_500DPS = 0x02, 328 | BNO055_CONF_GYRO_RANGE_250DPS = 0x03, 329 | BNO055_CONF_GYRO_RANGE_125DPS = 0x04 330 | } bno055_gyro_range_t; 331 | 332 | typedef enum { 333 | BNO055_CONF_GYRO_BANDWIDTH_523HZ = 0x00, 334 | BNO055_CONF_GYRO_BANDWIDTH_230HZ = 0x08, 335 | BNO055_CONF_GYRO_BANDWIDTH_116HZ = 0x10, 336 | BNO055_CONF_GYRO_BANDWIDTH_47HZ = 0x18, 337 | BNO055_CONF_GYRO_BANDWIDTH_23HZ = 0x20, 338 | BNO055_CONF_GYRO_BANDWIDTH_12HZ = 0x28, 339 | BNO055_CONF_GYRO_BANDWIDTH_64HZ = 0x30, 340 | BNO055_CONF_GYRO_BANDWIDTH_32HZ = 0x38 341 | } bno055_gyro_bandwidth_t; 342 | 343 | typedef enum { 344 | BNO055_CONF_GYRO_MODE_NORMAL = 0x00, 345 | BNO055_CONF_GYRO_MODE_FAST_PWR_UP = 0x01, 346 | BNO055_CONF_GYRO_MODE_DEEP_SUSPEND = 0x02, 347 | BNO055_CONF_GYRO_MODE_SUSPEND = 0x03, 348 | BNO055_CONF_GYRO_MODE_ADVANCED_PWR_SAVE = 0x04 349 | } bno055_gyro_mode_t; 350 | 351 | typedef enum { 352 | BNO055_CONF_MAG_RATE_2HZ = 0x00, 353 | BNO055_CONF_MAG_RATE_6HZ = 0x01, 354 | BNO055_CONF_MAG_RATE_8HZ = 0x02, 355 | BNO055_CONF_MAG_RATE_10HZ = 0x03, 356 | BNO055_CONF_MAG_RATE_15HZ = 0x04, 357 | BNO055_CONF_MAG_RATE_20HZ = 0x05, 358 | BNO055_CONF_MAG_RATE_25HZ = 0x06, 359 | BNO055_CONF_MAG_RATE_30HZ = 0x07 360 | } bno055_mag_rate_t; 361 | 362 | typedef enum { 363 | BNO055_CONF_MAG_MODE_LOW_PWR = 0x00, 364 | BNO055_CONF_MAG_MODE_REGULAR = 0x08, 365 | BNO055_CONF_MAG_MODE_ENHANCED_REGULAR = 0x10, 366 | BNO055_CONF_MAG_MODE_HIGH_ACCURACY = 0x18 367 | } bno055_mag_mode_t; 368 | 369 | typedef enum { 370 | BNO055_CONF_MAG_PWRMODE_NORMAL = 0x00, 371 | BNO055_CONF_MAG_PWRMODE_SLEEP = 0x20, 372 | BNO055_CONF_MAG_PWRMODE_SUSPEND = 0x40, 373 | BNO055_CONF_MAG_PWRMODE_FORCED = 0x60 374 | } bno055_mag_pwrmode_t; 375 | 376 | typedef enum { 377 | BNO055_CONF_GYRO_AUTO_SLEEP_DURATION_4MS = 0x08, 378 | BNO055_CONF_GYRO_AUTO_SLEEP_DURATION_5MS = 0x10, 379 | BNO055_CONF_GYRO_AUTO_SLEEP_DURATION_8MS = 0x18, 380 | BNO055_CONF_GYRO_AUTO_SLEEP_DURATION_10MS = 0x20, 381 | BNO055_CONF_GYRO_AUTO_SLEEP_DURATION_15MS = 0x28, 382 | BNO055_CONF_GYRO_AUTO_SLEEP_DURATION_20MS = 0x30, 383 | BNO055_CONF_GYRO_AUTO_SLEEP_DURATION_40MS = 0x38 384 | } bno055_gyro_auto_sleep_duration_t; 385 | 386 | typedef enum { 387 | BNO055_CONF_GYRO_SLEEP_DURATION_2MS = 0x00, 388 | BNO055_CONF_GYRO_SLEEP_DURATION_4MS = 0x01, 389 | BNO055_CONF_GYRO_SLEEP_DURATION_5MS = 0x02, 390 | BNO055_CONF_GYRO_SLEEP_DURATION_8MS = 0x03, 391 | BNO055_CONF_GYRO_SLEEP_DURATION_10MS = 0x04, 392 | BNO055_CONF_GYRO_SLEEP_DURATION_15MS = 0x05, 393 | BNO055_CONF_GYRO_SLEEP_DURATION_18MS = 0x06, 394 | BNO055_CONF_GYRO_SLEEP_DURATION_20MS = 0x07 395 | } bno055_gyro_sleep_duration_t; 396 | 397 | typedef enum { 398 | BNO055_CONF_ACCEL_SLEEP_DURATION_0_5MS = 0x00, 399 | BNO055_CONF_ACCEL_SLEEP_DURATION_1MS = 0x0C, 400 | BNO055_CONF_ACCEL_SLEEP_DURATION_2MS = 0x0E, 401 | BNO055_CONF_ACCEL_SLEEP_DURATION_4MS = 0x10, 402 | BNO055_CONF_ACCEL_SLEEP_DURATION_6MS = 0x12, 403 | BNO055_CONF_ACCEL_SLEEP_DURATION_10MS = 0x14, 404 | BNO055_CONF_ACCEL_SLEEP_DURATION_25MS = 0x16, 405 | BNO055_CONF_ACCEL_SLEEP_DURATION_50MS = 0x18, 406 | BNO055_CONF_ACCEL_SLEEP_DURATION_100MS = 0x1A, 407 | BNO055_CONF_ACCEL_SLEEP_DURATION_500MS = 0x1C, 408 | BNO055_CONF_ACCEL_SLEEP_DURATION_1000MS = 0x1E 409 | } bno055_accel_sleep_duration_t; 410 | 411 | typedef enum { 412 | BNO055_CONF_ACCEL_SLEEP_MODE_EVTDRIVEN = 0x00, 413 | BNO055_CONF_ACCEL_SLEEP_MODE_SAMPLING = 0x01 414 | } bno055_accel_sleep_mode_t; 415 | 416 | typedef struct { 417 | int16_t accelOffsetX = 0; 418 | int16_t accelOffsetY = 0; 419 | int16_t accelOffsetZ = 0; 420 | 421 | int16_t magOffsetX = 0; 422 | int16_t magOffsetY = 0; 423 | int16_t magOffsetZ = 0; 424 | 425 | int16_t gyroOffsetX = 0; 426 | int16_t gyroOffsetY = 0; 427 | int16_t gyroOffsetZ = 0; 428 | 429 | int16_t accelRadius = 0; 430 | int16_t magRadius = 0; 431 | } bno055_offsets_t; 432 | 433 | typedef struct { 434 | uint8_t sys = 0; 435 | uint8_t gyro = 0; 436 | uint8_t mag = 0; 437 | uint8_t accel = 0; 438 | } bno055_calibration_t; 439 | 440 | typedef struct { 441 | uint8_t accelNoSlowMotion = 0; 442 | uint8_t accelAnyMotion = 0; 443 | uint8_t accelHighG = 0; 444 | uint8_t gyroHR = 0; 445 | uint8_t gyroAnyMotion = 0; 446 | } bno055_interrupts_status_t; 447 | 448 | class BNO055BaseException : public std::exception { 449 | protected: 450 | std::string _msg; 451 | 452 | public: 453 | BNO055BaseException(std::string message = ":-(, an error is occurred.") { _msg = message; }; 454 | virtual const char *what() const throw() { return _msg.c_str(); } 455 | }; 456 | 457 | class BNO055ReadFail : public BNO055BaseException { 458 | public: 459 | BNO055ReadFail(std::string message = 460 | "(!*)this is specified in datasheet, but it is not in UART Application note, so it doesn't have an " 461 | "official description.") 462 | : BNO055BaseException(message){}; 463 | }; 464 | 465 | class BNO055WriteFail : public BNO055BaseException { 466 | public: 467 | BNO055WriteFail(std::string message = "Check connection, protocol settings and operation mode of the BNO055.") 468 | : BNO055BaseException(message){}; 469 | }; 470 | 471 | class BNO055RegmapInvalidAddress : public BNO055BaseException { 472 | public: 473 | BNO055RegmapInvalidAddress( 474 | std::string message = "Check the if the register is addressable. example in Page 0, should be from 0x38 to 0x6A.") 475 | : BNO055BaseException(message){}; 476 | }; 477 | 478 | class BNO055RegmapWriteDisabled : public BNO055BaseException { 479 | public: 480 | BNO055RegmapWriteDisabled(std::string message = "Check the property of register.") : BNO055BaseException(message){}; 481 | }; 482 | 483 | class BNO055WrongStartByte : public BNO055BaseException { 484 | public: 485 | BNO055WrongStartByte(std::string message = "Check if the first byte sent is 0xAA.") : BNO055BaseException(message){}; 486 | }; 487 | 488 | class BNO055BusOverRunError : public BNO055BaseException { 489 | public: 490 | BNO055BusOverRunError(std::string message = "Resend the command") : BNO055BaseException(message){}; 491 | }; 492 | 493 | class BNO055MaxLengthError : public BNO055BaseException { 494 | public: 495 | BNO055MaxLengthError(std::string message = "Split the command,a single frame must have < 128 Bytes.") 496 | : BNO055BaseException(message){}; 497 | }; 498 | 499 | class BNO055MinLengthError : public BNO055BaseException { 500 | public: 501 | BNO055MinLengthError(std::string message = "Send a valid frame.") : BNO055BaseException(message){}; 502 | }; 503 | 504 | class BNO055ReceiveCharacterTimeout : public BNO055BaseException { 505 | public: 506 | BNO055ReceiveCharacterTimeout(std::string message = "Decrease waiting time between sending of two bytes of one frame.") 507 | : BNO055BaseException(message){}; 508 | }; 509 | 510 | class BNO055UnknowError : public BNO055BaseException { 511 | public: 512 | BNO055UnknowError(std::string message = ".") : BNO055BaseException(message){}; 513 | }; 514 | 515 | class BNO055UartTimeout : public BNO055BaseException { 516 | public: 517 | BNO055UartTimeout(std::string message = "timeout expired, if you see this often, try to increase timeoutMS.") 518 | : BNO055BaseException(message){}; 519 | }; 520 | 521 | class BNO055UartInitFailed : public BNO055BaseException { 522 | public: 523 | BNO055UartInitFailed(std::string message = "ESP32's UART Interface cannot be initialized.") : BNO055BaseException(message){}; 524 | }; 525 | 526 | class BNO055ChipNotDetected : public BNO055BaseException { 527 | public: 528 | BNO055ChipNotDetected(std::string message = "Check your wiring.") : BNO055BaseException(message){}; 529 | }; 530 | 531 | class BNO055WrongOprMode : public BNO055BaseException { 532 | public: 533 | BNO055WrongOprMode(std::string message = "Check the OperationMode.") : BNO055BaseException(message){}; 534 | }; 535 | 536 | class BNO055I2CError : public BNO055BaseException { 537 | public: 538 | BNO055I2CError(std::string message = "I2CError: Check your wiring.") : BNO055BaseException(message){}; 539 | }; 540 | 541 | class BNO055 { 542 | public: 543 | BNO055(i2c_port_t i2cPort, uint8_t i2cAddr, gpio_num_t rstPin = GPIO_NUM_MAX, gpio_num_t intPin = GPIO_NUM_MAX); 544 | BNO055(uart_port_t uartPort, gpio_num_t txPin = GPIO_NUM_17, gpio_num_t rxPin = GPIO_NUM_16, gpio_num_t rstPin = GPIO_NUM_MAX, 545 | gpio_num_t intPin = GPIO_NUM_MAX); 546 | ~BNO055(); 547 | void begin(); 548 | void stop(); 549 | void reset(); 550 | 551 | void setPage(uint8_t page, bool forced = false); 552 | 553 | void setPwrModeNormal(); 554 | void setPwrModeLowPower(); 555 | void setPwrModeSuspend(); 556 | 557 | void setOprModeConfig(bool forced = false); 558 | void setOprModeAccOnly(bool forced = false); 559 | void setOprModeMagOnly(bool forced = false); 560 | void setOprModeGyroOnly(bool forced = false); 561 | void setOprModeAccMag(bool forced = false); 562 | void setOprModeAccGyro(bool forced = false); 563 | void setOprModeMagGyro(bool forced = false); 564 | void setOprModeAMG(bool forced = false); 565 | void setOprModeIMU(bool forced = false); 566 | void setOprModeCompass(bool forced = false); 567 | void setOprModeM4G(bool forced = false); 568 | void setOprModeNdofFmcOff(bool forced = false); 569 | void setOprModeNdof(bool forced = false); 570 | 571 | void enableExternalCrystal(); 572 | void disableExternalCrystal(); 573 | 574 | bno055_offsets_t getSensorOffsets(); 575 | void setSensorOffsets(bno055_offsets_t newOffsets); 576 | bno055_calibration_t getCalibration(); 577 | 578 | int8_t getTemp(); 579 | 580 | bno055_vector_t getVectorAccelerometer(); 581 | bno055_vector_t getVectorMagnetometer(); 582 | bno055_vector_t getVectorGyroscope(); 583 | bno055_vector_t getVectorEuler(); 584 | bno055_vector_t getVectorLinearAccel(); 585 | bno055_vector_t getVectorGravity(); 586 | bno055_quaternion_t getQuaternion(); 587 | 588 | int16_t getSWRevision(); 589 | uint8_t getBootloaderRevision(); 590 | 591 | bno055_system_status_t getSystemStatus(); 592 | bno055_self_test_result_t getSelfTestResult(); 593 | bno055_system_error_t getSystemError(); 594 | 595 | void setAxisRemap(bno055_axis_config_t config = BNO055_REMAP_CONFIG_P1, bno055_axis_sign_t sign = BNO055_REMAP_SIGN_P1); 596 | void setUnits(bno055_accel_unit_t accel = BNO055_UNIT_ACCEL_MS2, 597 | bno055_angular_rate_unit_t angularRate = BNO055_UNIT_ANGULAR_RATE_RPS, 598 | bno055_euler_unit_t euler = BNO055_UNIT_EULER_DEGREES, bno055_temperature_unit_t temp = BNO055_UNIT_TEMP_C, 599 | bno055_data_output_format_t format = BNO055_DATA_FORMAT_ANDROID); 600 | 601 | void setAccelConfig(bno055_accel_range_t range = BNO055_CONF_ACCEL_RANGE_4G, 602 | bno055_accel_bandwidth_t bandwidth = BNO055_CONF_ACCEL_BANDWIDTH_62_5HZ, 603 | bno055_accel_mode_t mode = BNO055_CONF_ACCEL_MODE_NORMAL); 604 | void setGyroConfig(bno055_gyro_range_t range = BNO055_CONF_GYRO_RANGE_2000DPS, 605 | bno055_gyro_bandwidth_t bandwidth = BNO055_CONF_GYRO_BANDWIDTH_32HZ, 606 | bno055_gyro_mode_t mode = BNO055_CONF_GYRO_MODE_NORMAL); 607 | void setMagConfig(bno055_mag_rate_t rate = BNO055_CONF_MAG_RATE_20HZ, 608 | bno055_mag_pwrmode_t pwrmode = BNO055_CONF_MAG_PWRMODE_FORCED, 609 | bno055_mag_mode_t mode = BNO055_CONF_MAG_MODE_REGULAR); 610 | 611 | void setGyroSleepConfig(bno055_gyro_auto_sleep_duration_t autoSleepDuration, bno055_gyro_sleep_duration_t sleepDuration); 612 | void setAccelSleepConfig(bno055_accel_sleep_duration_t sleepDuration, bno055_accel_sleep_mode_t sleepMode); 613 | 614 | void enableAccelSlowMotionInterrupt(bool useInterruptPin = true); 615 | void setAccelSlowMotionInterrupt(uint8_t threshold, uint8_t duration, bool xAxis = true, bool yAxis = true, 616 | bool zAxis = true); 617 | void disableAccelSlowMotionInterrupt(); 618 | 619 | void enableAccelNoMotionInterrupt(bool useInterruptPin = true); 620 | void setAccelNoMotionInterrupt(uint8_t threshold, uint8_t duration, bool xAxis = true, bool yAxis = true, bool zAxis = true); 621 | void disableAccelNoMotionInterrupt(); 622 | 623 | void enableAccelAnyMotionInterrupt(bool useInterruptPin = true); 624 | void setAccelAnyMotionInterrupt(uint8_t threshold, uint8_t duration, bool xAxis = true, bool yAxis = true, bool zAxis = true); 625 | void disableAccelAnyMotionInterrupt(); 626 | 627 | void enableAccelHighGInterrupt(bool useInterruptPin = true); 628 | void setAccelHighGInterrupt(uint8_t threshold, uint8_t duration, bool xAxis = true, bool yAxis = true, bool zAxis = true); 629 | void disableAccelHighGInterrupt(); 630 | 631 | void enableGyroAnyMotionInterrupt(bool useInterruptPin = true); 632 | void setGyroAnyMotionInterrupt(uint8_t threshold, uint8_t slopeSamples, uint8_t awakeDuration, bool xAxis = true, 633 | bool yAxis = true, bool zAxis = true, bool filtered = true); 634 | void disableGyroAnyMotionInterrupt(); 635 | 636 | void enableGyroHRInterrupt(bool useInterruptPin = true); 637 | void setGyroHRInterrupt(uint8_t thresholdX, uint8_t duration, uint8_t hysteresisX, uint8_t thresholdY, uint8_t durationY, 638 | uint8_t hysteresisY, uint8_t thresholdZ, uint8_t durationZ, uint8_t hysteresisZ, bool xAxis = true, 639 | bool yAxis = true, bool zAxis = true, bool filtered = true); 640 | void disableGyroHRInterrupt(); 641 | 642 | bno055_interrupts_status_t getInterruptsStatus(); 643 | void clearInterruptPin(); 644 | static void IRAM_ATTR bno055_interrupt_handler(void *arg); 645 | 646 | std::exception getException(uint8_t errcode); 647 | 648 | void i2c_readLen(uint8_t reg, uint8_t *buffer, uint8_t len, uint32_t timeoutMS); 649 | void i2c_writeLen(uint8_t reg, uint8_t *buffer, uint8_t len, uint32_t timeoutMS); 650 | 651 | void uart_readLen(bno055_reg_t reg, uint8_t *buffer, uint8_t len, uint32_t timeoutMS); 652 | void uart_writeLen(bno055_reg_t reg, uint8_t *data, uint8_t len, uint32_t timeoutMS); 653 | 654 | void readLen(bno055_reg_t reg, uint8_t *buffer, uint8_t len = 1, uint8_t page = 0, uint32_t timeoutMS = DEFAULT_UART_TIMEOUT_MS); 655 | void writeLen(bno055_reg_t reg, uint8_t *data, uint8_t len = 1, uint8_t page = 0, uint32_t timeoutMS = DEFAULT_UART_TIMEOUT_MS); 656 | 657 | bool interruptFlag = false; 658 | 659 | protected: 660 | const uart_config_t uart_config = {.baud_rate = 115200, 661 | .data_bits = UART_DATA_8_BITS, 662 | .parity = UART_PARITY_DISABLE, 663 | .stop_bits = UART_STOP_BITS_1, 664 | .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, 665 | .rx_flow_ctrl_thresh = 0, 666 | .source_clk = UART_SCLK_DEFAULT}; 667 | 668 | typedef enum { 669 | BNO055_VECTOR_ACCELEROMETER = 0x08, // Default: m/s² 670 | BNO055_VECTOR_MAGNETOMETER = 0x0E, // Default: uT 671 | BNO055_VECTOR_GYROSCOPE = 0x14, // Default: rad/s 672 | BNO055_VECTOR_EULER = 0x1A, // Default: degrees 673 | BNO055_VECTOR_LINEARACCEL = 0x28, // Default: m/s² 674 | BNO055_VECTOR_GRAVITY = 0x2E // Default: m/s² 675 | } bno055_vector_type_t; 676 | 677 | typedef enum { 678 | BNO055_OPERATION_MODE_CONFIG = 0x00, 679 | BNO055_OPERATION_MODE_ACCONLY = 0x01, 680 | BNO055_OPERATION_MODE_MAGONLY = 0x02, 681 | BNO055_OPERATION_MODE_GYRONLY = 0x03, 682 | BNO055_OPERATION_MODE_ACCMAG = 0x04, 683 | BNO055_OPERATION_MODE_ACCGYRO = 0x05, 684 | BNO055_OPERATION_MODE_MAGGYRO = 0x06, 685 | BNO055_OPERATION_MODE_AMG = 0x07, 686 | BNO055_OPERATION_MODE_IMU = 0x08, 687 | BNO055_OPERATION_MODE_COMPASS = 0x09, 688 | BNO055_OPERATION_MODE_M4G = 0x0A, 689 | BNO055_OPERATION_MODE_NDOF_FMC_OFF = 0x0B, 690 | BNO055_OPERATION_MODE_NDOF = 0x0C 691 | } bno055_opmode_t; 692 | 693 | uint8_t UART_ROUND_NUM = 64; 694 | 695 | gpio_num_t _rstPin; 696 | gpio_num_t _intPin; 697 | 698 | uint8_t _page = 0; 699 | 700 | bool _i2cFlag; 701 | 702 | uart_port_t _uartPort; 703 | i2c_port_t _i2cPort; 704 | uint8_t _i2cAddr; 705 | 706 | gpio_num_t _txPin; 707 | gpio_num_t _rxPin; 708 | 709 | uint16_t accelScale = 100; 710 | uint16_t tempScale = 1; 711 | uint16_t angularRateScale = 16; 712 | uint16_t eulerScale = 16; 713 | uint16_t magScale = 16; 714 | 715 | bno055_opmode_t _mode; 716 | void setOprMode(bno055_opmode_t mode, bool forced = false); 717 | 718 | void setPwrMode(bno055_powermode_t pwrMode); 719 | 720 | void setExtCrystalUse(bool state); 721 | 722 | bno055_vector_t getVector(bno055_vector_type_t vec); 723 | void enableInterrupt(uint8_t flag, bool useInterruptPin = true); 724 | void disableInterrupt(uint8_t flag); 725 | }; 726 | 727 | #endif --------------------------------------------------------------------------------