diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 72ce416e4f..ed569c2c41 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -99,6 +99,8 @@ jobs: target: esp32 - path: 'components/bmi270/example' target: esp32p4 + - path: 'components/bno055/example' + target: esp32p4 - path: 'components/bq27220/example' target: esp32 - path: 'components/button/example' diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index 75ac4dd2ab..cec691bdd2 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -73,6 +73,7 @@ jobs: components/ble_gatt_server components/bm8563 components/bmi270 + components/bno055 components/button components/byte90 components/canopen diff --git a/components/bno055/CMakeLists.txt b/components/bno055/CMakeLists.txt new file mode 100644 index 0000000000..17d4b59e0f --- /dev/null +++ b/components/bno055/CMakeLists.txt @@ -0,0 +1,5 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src" + REQUIRES "base_peripheral" + ) diff --git a/components/bno055/README.md b/components/bno055/README.md new file mode 100644 index 0000000000..ecc495fa1c --- /dev/null +++ b/components/bno055/README.md @@ -0,0 +1,27 @@ +# BNO055 9-Axis Absolute Orientation IMU Component + +[![Badge](https://components.espressif.com/components/espp/bno055/badge.svg)](https://components.espressif.com/components/espp/bno055) + +The `Bno055` component provides a driver for the Bosch BNO055 9-axis absolute +orientation sensor over I2C. The BNO055 runs sensor fusion on its own +microcontroller, so the driver reads finished orientation data rather than +filtering raw samples on the ESP. + +It supports configuring and reading: +* Operating mode (raw-only modes and the IMU, COMPASS, M4G, and NDOF fusion + modes) +* 3-axis acceleration, angular rate, and magnetic field +* Fused orientation as Euler angles and as a quaternion +* Linear acceleration (gravity removed) and the gravity vector +* Die temperature and per-sensor calibration status + +All outputs are read in a single 46-byte I2C burst and reported in SI units +(m/s^2, rad/s, rad, uT). + +It does not _yet_ support saving and restoring calibration offsets, axis +remapping, or the interrupt pin. + +## Example + +The [example](./example) shows how to use the `espp::Bno055` component to +initialize a BNO055 and print its outputs. diff --git a/components/bno055/example/CMakeLists.txt b/components/bno055/example/CMakeLists.txt new file mode 100644 index 0000000000..b3a363ad3a --- /dev/null +++ b/components/bno055/example/CMakeLists.txt @@ -0,0 +1,22 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +# add the component directories that we want to use +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py i2c bno055 task" + CACHE STRING + "List of components to include" + ) + +project(bno055_example) + +set(CMAKE_CXX_STANDARD 23) diff --git a/components/bno055/example/README.md b/components/bno055/example/README.md new file mode 100644 index 0000000000..546bb58791 --- /dev/null +++ b/components/bno055/example/README.md @@ -0,0 +1,41 @@ +# BNO055 Example + +This example shows how to use the `espp::Bno055` component to initialize a +BNO055 9-axis absolute orientation IMU over I2C and print its outputs. + +It puts the BNO055 in IMU fusion mode (accelerometer + gyroscope, heading +relative to power-on), reads all outputs at 100 Hz in one I2C burst per sample, +and prints one CSV row per sample: acceleration, angular rate, Euler angles, the +orientation quaternion, temperature, and calibration status. The header line +starts with `%` so the output can be pasted straight into a plotter. + +## How to use example + +### Hardware Required + +This example runs on any ESP32 series device with a BNO055 breakout connected +over I2C. The defaults are SDA=GPIO7 and SCL=GPIO8 on the ESP32-P4 (the +internal I2C bus pins on the espp `esp32-p4-eth` board) and SDA=GPIO21, +SCL=GPIO22 on other targets. Change the pins and the I2C address (0x28 or 0x29) +with `idf.py menuconfig` under `Example Configuration`. + +### Build and Flash + +Build the project and flash it to the board, then run monitor tool to view +serial output: + +``` +idf.py -p PORT flash monitor +``` + +(Replace PORT with the name of the serial port to use.) + +(To exit the serial monitor, type ``Ctrl-]``.) + +See the Getting Started Guide for full steps to configure and use ESP-IDF to build projects. + +## Example Output + +``` +% Time (s), Accel X (m/s^2), Accel Y (m/s^2), Accel Z (m/s^2), Gyro X (rad/s), Gyro Y (rad/s), Gyro Z (rad/s), Heading (rad), Roll (rad), Pitch (rad), Quat W, Quat X, Quat Y, Quat Z, Temp (C), Calib Sys, Calib Gyro, Calib Accel, Calib Mag +``` diff --git a/components/bno055/example/main/CMakeLists.txt b/components/bno055/example/main/CMakeLists.txt new file mode 100644 index 0000000000..a941e22ba7 --- /dev/null +++ b/components/bno055/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/bno055/example/main/Kconfig.projbuild b/components/bno055/example/main/Kconfig.projbuild new file mode 100644 index 0000000000..13fc73148a --- /dev/null +++ b/components/bno055/example/main/Kconfig.projbuild @@ -0,0 +1,33 @@ +menu "Example Configuration" + +config EXAMPLE_I2C_SDA_GPIO + int "SDA GPIO Num" + range 0 50 + default 7 if IDF_TARGET_ESP32P4 + default 21 + help + GPIO number for I2C Master data line. + +config EXAMPLE_I2C_SCL_GPIO + int "SCL GPIO Num" + range 0 50 + default 8 if IDF_TARGET_ESP32P4 + default 22 + help + GPIO number for I2C Master clock line. + +config EXAMPLE_I2C_CLOCK_SPEED_HZ + int "I2C Clock Speed" + range 100 400000 + default 400000 + help + I2C clock speed in Hz. The BNO055 supports up to 400 kHz. + +config EXAMPLE_I2C_ADDRESS + hex "BNO055 I2C Address" + range 0x28 0x29 + default 0x28 + help + I2C address of the BNO055: 0x28 with ADR (COM3) low, 0x29 with it high. + +endmenu diff --git a/components/bno055/example/main/bno055_example.cpp b/components/bno055/example/main/bno055_example.cpp new file mode 100644 index 0000000000..e74ee73b95 --- /dev/null +++ b/components/bno055/example/main/bno055_example.cpp @@ -0,0 +1,102 @@ +#include +#include + +#include "bno055.hpp" +#include "i2c.hpp" +#include "task.hpp" + +using namespace std::chrono_literals; + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "BNO055 Example", .level = espp::Logger::Verbosity::INFO}); + logger.info("Starting example!"); + + //! [bno055 example] + // make the i2c we'll use to communicate + static constexpr auto i2c_port = I2C_NUM_0; + static constexpr auto i2c_clock_speed = CONFIG_EXAMPLE_I2C_CLOCK_SPEED_HZ; + static constexpr gpio_num_t i2c_sda = (gpio_num_t)CONFIG_EXAMPLE_I2C_SDA_GPIO; + static constexpr gpio_num_t i2c_scl = (gpio_num_t)CONFIG_EXAMPLE_I2C_SCL_GPIO; + logger.info("Creating I2C on port {} with SDA {} and SCL {}", i2c_port, i2c_sda, i2c_scl); + espp::I2c i2c({.port = i2c_port, + .sda_io_num = i2c_sda, + .scl_io_num = i2c_scl, + .sda_pullup_en = GPIO_PULLUP_ENABLE, + .scl_pullup_en = GPIO_PULLUP_ENABLE, + .clk_speed = i2c_clock_speed}); + + static constexpr uint8_t address = CONFIG_EXAMPLE_I2C_ADDRESS; + std::error_code ec; + auto imu_device = + i2c.add_device({.device_address = address, + .timeout_ms = static_cast(i2c.config().timeout_ms), + .scl_speed_hz = i2c.config().clk_speed, + .log_level = espp::Logger::Verbosity::WARN}, + ec); + if (!imu_device) { + logger.error("Failed to create BNO055 I2C device: {}", ec.message()); + return; + } + + // create the IMU; auto_init resets it and enters IMU (accel + gyro) fusion + // mode, which takes about 700 ms + espp::Bno055 imu({.device_address = address, + .probe = espp::make_i2c_addressed_probe(imu_device), + .write = espp::make_i2c_addressed_write(imu_device), + .write_then_read = espp::make_i2c_addressed_write_then_read(imu_device), + .mode = espp::Bno055::OperatingMode::IMU, + .use_external_crystal = true, + .auto_init = true, + .log_level = espp::Logger::Verbosity::INFO}); + if (!imu.initialized()) { + logger.error("BNO055 not found at address {:#04x}", address); + return; + } + + // print the header for the IMU data (for plotting) + fmt::print("% Time (s), " + "Accel X (m/s^2), Accel Y (m/s^2), Accel Z (m/s^2), " + "Gyro X (rad/s), Gyro Y (rad/s), Gyro Z (rad/s), " + "Heading (rad), Roll (rad), Pitch (rad), " + "Quat W, Quat X, Quat Y, Quat Z, " + "Temp (C), Calib Sys, Calib Gyro, Calib Accel, Calib Mag\n"); + + // read the IMU at 100 Hz and print one CSV row per sample + auto task_fn = [&](std::mutex &m, std::condition_variable &cv) { + auto start = std::chrono::high_resolution_clock::now(); + + std::error_code ec; + if (!imu.update(ec)) { + logger.error("Failed to update BNO055: {}", ec.message()); + } else { + auto data = imu.data(); + std::string text = fmt::format("{:.3f},", esp_timer_get_time() / 1'000'000.0f); + text += fmt::format("{:.3f},{:.3f},{:.3f},", data.accel_mps2.x, data.accel_mps2.y, + data.accel_mps2.z); + text += fmt::format("{:.4f},{:.4f},{:.4f},", data.gyro_radps.x, data.gyro_radps.y, + data.gyro_radps.z); + text += fmt::format("{:.4f},{:.4f},{:.4f},", data.euler.heading, data.euler.roll, + data.euler.pitch); + text += fmt::format("{:.4f},{:.4f},{:.4f},{:.4f},", data.orientation.w, data.orientation.x, + data.orientation.y, data.orientation.z); + text += fmt::format("{},{},{},{},{}", data.temperature_c, data.calibration.system, + data.calibration.gyro, data.calibration.accel, data.calibration.mag); + fmt::print("{}\n", text); + } + + std::unique_lock lk(m); + cv.wait_until(lk, start + 10ms); + return false; + }; + + espp::Task task({.callback = task_fn, + .task_config = {.name = "BNO055 Task", .stack_size_bytes = 6 * 1024}, + .log_level = espp::Logger::Verbosity::WARN}); + task.start(); + + // loop forever + while (true) { + std::this_thread::sleep_for(1s); + } + //! [bno055 example] +} diff --git a/components/bno055/example/sdkconfig.defaults b/components/bno055/example/sdkconfig.defaults new file mode 100644 index 0000000000..cac62acdff --- /dev/null +++ b/components/bno055/example/sdkconfig.defaults @@ -0,0 +1,14 @@ +CONFIG_IDF_TARGET="esp32p4" + +CONFIG_FREERTOS_HZ=1000 + +# set compiler optimization level to -O2 (compile for performance) +CONFIG_COMPILER_OPTIMIZATION_PERF=y + +# Common ESP-related +# +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=16384 + +# Set esp-timer task stack size to 6KB +CONFIG_ESP_TIMER_TASK_STACK_SIZE=6144 diff --git a/components/bno055/example/sdkconfig.defaults.esp32p4 b/components/bno055/example/sdkconfig.defaults.esp32p4 new file mode 100644 index 0000000000..d36ab74fd9 --- /dev/null +++ b/components/bno055/example/sdkconfig.defaults.esp32p4 @@ -0,0 +1,11 @@ +# Applied on top of sdkconfig.defaults when the target is esp32p4 (ESP-IDF +# picks up sdkconfig.defaults. automatically). +# +# Support pre-v3 silicon (e.g. ESP32-P4 v1.x on the ESP32-P4-ETH and +# Function-EV boards). +CONFIG_ESP32P4_SELECTS_REV_LESS_V3=y + +# The ESP-IDF 6.1 esp32p4 bootloader (0x6140 bytes) no longer fits in front of +# the default partition-table offset 0x8000 (max 0x6000 bytes there); move the +# table to 0x9000 like the other esp32p4 boards in this repo. +CONFIG_PARTITION_TABLE_OFFSET=0x9000 diff --git a/components/bno055/idf_component.yml b/components/bno055/idf_component.yml new file mode 100644 index 0000000000..b94d086ca8 --- /dev/null +++ b/components/bno055/idf_component.yml @@ -0,0 +1,20 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "BNO055 9-Axis absolute orientation IMU component for ESP-IDF" +url: "https://github.com/esp-cpp/espp/tree/main/components/bno055" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/imu/bno055.html" +examples: + - path: example +tags: + - cpp + - Component + - BNO055 + - IMU + - Peripheral +dependencies: + idf: + version: '>=5.0' + espp/base_peripheral: '>=1.0' diff --git a/components/bno055/include/bno055.hpp b/components/bno055/include/bno055.hpp new file mode 100644 index 0000000000..589b4ead2a --- /dev/null +++ b/components/bno055/include/bno055.hpp @@ -0,0 +1,172 @@ +#pragma once + +#include +#include +#include +#include + +#include "base_peripheral.hpp" + +namespace espp { + +/// @brief Driver for the Bosch BNO055 9-axis absolute orientation sensor over I2C. +/// +/// The BNO055 combines a 3-axis accelerometer, 3-axis gyroscope, and 3-axis +/// magnetometer with an on-chip Cortex-M0 that runs Bosch's sensor fusion and +/// outputs orientation (Euler angles and a quaternion), linear acceleration, +/// and gravity. +/// +/// This driver configures the sensor into one of its operating modes and reads +/// all sensor and fusion outputs in a single 46-byte burst. Units are set to +/// SI: m/s^2 for acceleration, rad/s for angular rate, rad for Euler angles, +/// and uT for the magnetic field. +/// +/// The default mode is IMU (accelerometer + gyroscope fusion, heading relative +/// to power-on). NDOF adds the magnetometer for an absolute heading, but is +/// sensitive to nearby motors and ferrous metal. +/// +/// \note The BNO055 stretches the I2C clock; keep the I2C timeout at or above +/// the espp default (10 ms). +/// +/// Datasheet: +/// - https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf +/// +/// \section bno055_example Example +/// \snippet bno055_example.cpp bno055 example +class Bno055 : public BasePeripheral { +public: + static constexpr uint8_t DEFAULT_ADDRESS = 0x28; ///< Address with ADR (COM3) pulled low. + static constexpr uint8_t ALTERNATE_ADDRESS = 0x29; ///< Address with ADR (COM3) pulled high. + static constexpr uint8_t CHIP_ID = 0xA0; ///< Expected value of the CHIP_ID register. + + /// @brief Sensor operating mode (OPR_MODE register). + enum class OperatingMode : uint8_t { + CONFIG = 0x00, ///< Configuration mode; no outputs are updated. + ACC_ONLY = 0x01, ///< Accelerometer only. + ACC_GYRO = 0x05, ///< Accelerometer and gyroscope, no fusion. + AMG = 0x07, ///< All three sensors, no fusion. + IMU = 0x08, ///< Accel + gyro fusion; heading is relative to power-on. + COMPASS = 0x09, ///< Accel + mag fusion. + M4G = 0x0A, ///< Accel + mag fusion, mag used in place of gyro. + NDOF_FMC_OFF = 0x0B, ///< Full 9-DoF fusion without fast mag calibration. + NDOF = 0x0C, ///< Full 9-DoF fusion; heading is absolute (magnetic north). + }; + + /// @brief Calibration status of each sensor, 0 (uncalibrated) to 3 (fully calibrated). + struct CalibrationStatus { + uint8_t system{0}; ///< Overall fusion calibration. + uint8_t gyro{0}; ///< Gyroscope calibration. + uint8_t accel{0}; ///< Accelerometer calibration. + uint8_t mag{0}; ///< Magnetometer calibration. + }; + + /// @brief Three-axis vector. + struct Vector3 { + float x{0.0f}; ///< X component. + float y{0.0f}; ///< Y component. + float z{0.0f}; ///< Z component. + }; + + /// @brief Unit quaternion. + struct Quaternion { + float w{1.0f}; ///< Scalar component. + float x{0.0f}; ///< X component. + float y{0.0f}; ///< Y component. + float z{0.0f}; ///< Z component. + }; + + /// @brief Euler angles, in radians. + struct Euler { + float heading{0.0f}; ///< Yaw, 0 to 2*pi. + float roll{0.0f}; ///< Roll, -pi/2 to pi/2. + float pitch{0.0f}; ///< Pitch, -pi to pi. + }; + + /// @brief One complete set of sensor and fusion outputs. + struct Data { + Vector3 accel_mps2; ///< Raw acceleration including gravity. + Vector3 mag_ut; ///< Magnetic field, in microtesla. + Vector3 gyro_radps; ///< Angular rate. + Euler euler; ///< Fused orientation as Euler angles. + Quaternion orientation; ///< Fused orientation as a quaternion. + Vector3 linear_accel_mps2; ///< Acceleration with gravity removed. + Vector3 gravity_mps2; ///< Gravity vector. + int8_t temperature_c{0}; ///< Die temperature, in degrees Celsius. + CalibrationStatus calibration; ///< Per-sensor calibration status. + }; + + /// @brief Driver configuration. + struct Config { + uint8_t device_address{DEFAULT_ADDRESS}; ///< I2C address of the sensor. + BasePeripheral::probe_fn probe{nullptr}; ///< Probe the I2C address. + BasePeripheral::write_fn write{nullptr}; ///< Write bytes to the sensor. + BasePeripheral::write_then_read_fn write_then_read{nullptr}; ///< Write then read. + OperatingMode mode{OperatingMode::IMU}; ///< Fusion mode to run in. + bool use_external_crystal{true}; ///< Use the board's 32 kHz crystal. + bool auto_init{true}; ///< Call init() on construction. + espp::Logger::Verbosity log_level{espp::Logger::Verbosity::WARN}; ///< Log verbosity. + }; + + /// @brief Construct the driver. + /// @param config Bus access functions and sensor settings. + explicit Bno055(const Config &config); + + /// @brief Reset the sensor, verify its chip ID, set SI units, and enter the configured mode. + /// @param ec Set on any bus error or if the chip ID does not match. + /// @return True if the sensor is running in the configured mode. + /// @note Blocks for roughly 700 ms while the sensor resets. + bool init(std::error_code &ec); + + /// @brief Read all sensor and fusion outputs in one burst and cache them. + /// @param ec Set on a bus error. + /// @return True if the read succeeded. + bool update(std::error_code &ec); + + /// @brief Get the most recent outputs read by update(). + /// @return A copy of the cached data. + Data data() const; + + /// @brief Check whether init() completed successfully. + /// @return True if the sensor is initialized. + bool initialized() const { return initialized_; } + + /// @brief Read the SYS_STATUS and SYS_ERR registers. + /// @param status Set to SYS_STATUS (5 = fusion running, 1 = system error). + /// @param error Set to SYS_ERR (0 = no error). + /// @param ec Set on a bus error. + void read_system_status(uint8_t &status, uint8_t &error, std::error_code &ec); + +protected: + /// Register addresses on page 0 (datasheet section 4.2). + enum class Register : uint8_t { + CHIP_ID = 0x00, ///< Chip ID, reads 0xA0. + PAGE_ID = 0x07, ///< Register page select. + ACC_DATA_X_LSB = 0x08, ///< Start of the 46-byte data block through CALIB_STAT. + CALIB_STAT = 0x35, ///< Calibration status of each sensor. + ST_RESULT = 0x36, ///< Power-on self test result. + SYS_STATUS = 0x39, ///< System status. + SYS_ERR = 0x3A, ///< System error code. + UNIT_SEL = 0x3B, ///< Output unit selection. + OPR_MODE = 0x3D, ///< Operating mode. + PWR_MODE = 0x3E, ///< Power mode. + SYS_TRIGGER = 0x3F, ///< Reset, self test, and clock source. + }; + + /// Number of bytes from ACC_DATA_X_LSB up to and including CALIB_STAT. + static constexpr size_t DATA_BLOCK_SIZE = 0x35 - 0x08 + 1; + + /// Write OPR_MODE and wait the datasheet's mode switching time. + void set_mode(OperatingMode mode, std::error_code &ec); + /// Poll CHIP_ID until it reads 0xA0, for up to about 1 s. + bool wait_for_chip_id(std::error_code &ec); + /// Convert the raw data block into SI units. + static Data parse(const std::array &raw); + + OperatingMode mode_; ///< Mode entered at the end of init(). + bool use_external_crystal_; ///< Whether init() selects the external crystal. + bool initialized_{false}; ///< Set when init() succeeds. + mutable std::mutex data_mutex_; ///< Protects data_. + Data data_{}; ///< Latest outputs read by update(). +}; + +} // namespace espp diff --git a/components/bno055/src/bno055.cpp b/components/bno055/src/bno055.cpp new file mode 100644 index 0000000000..e1fa417432 --- /dev/null +++ b/components/bno055/src/bno055.cpp @@ -0,0 +1,187 @@ +#include "bno055.hpp" + +#include + +using namespace std::chrono_literals; + +namespace espp { + +namespace { + +// Scale factors with UNIT_SEL = m/s^2, rad/s, rad, Celsius (datasheet section 3.6.4). +constexpr float kAccelLsbPerMps2 = 100.0f; +constexpr float kMagLsbPerUt = 16.0f; +constexpr float kGyroLsbPerRadps = 900.0f; +constexpr float kEulerLsbPerRad = 900.0f; +constexpr float kQuaternionLsbPerUnit = 16384.0f; + +// UNIT_SEL: accel m/s^2 (bit0=0), gyro rad/s (bit1=1), Euler rad (bit2=1), +// temperature Celsius (bit4=0), Windows orientation convention (bit7=0). +constexpr uint8_t kUnitSelSi = 0b0000'0110; + +constexpr uint8_t kSysTriggerReset = 0x20; +constexpr uint8_t kSysTriggerExternalCrystal = 0x80; +constexpr uint8_t kPowerModeNormal = 0x00; + +int16_t to_i16(const uint8_t *bytes) { + return static_cast(static_cast(bytes[0]) | + (static_cast(bytes[1]) << 8)); +} + +Bno055::Vector3 to_vector(const uint8_t *bytes, float lsb_per_unit) { + return {to_i16(bytes) / lsb_per_unit, to_i16(bytes + 2) / lsb_per_unit, + to_i16(bytes + 4) / lsb_per_unit}; +} + +} // namespace + +Bno055::Bno055(const Config &config) + : BasePeripheral({.address = config.device_address, + .probe = config.probe, + .write = config.write, + .write_then_read = config.write_then_read}, + "Bno055", config.log_level) + , mode_(config.mode) + , use_external_crystal_(config.use_external_crystal) { + if (config.auto_init) { + std::error_code ec; + if (!init(ec)) { + logger_.error("Failed to initialize BNO055: {}", ec.message()); + } + } +} + +bool Bno055::init(std::error_code &ec) { + initialized_ = false; + + // The sensor may still be booting (650 ms after power-on), so poll the chip ID. + if (!wait_for_chip_id(ec)) { + return false; + } + + set_mode(OperatingMode::CONFIG, ec); + if (ec) { + return false; + } + + write_u8_to_register(static_cast(Register::SYS_TRIGGER), kSysTriggerReset, ec); + // The sensor NACKs while resetting, so a write error here is expected. + ec.clear(); + std::this_thread::sleep_for(650ms); + if (!wait_for_chip_id(ec)) { + return false; + } + + write_u8_to_register(static_cast(Register::PWR_MODE), kPowerModeNormal, ec); + if (ec) { + return false; + } + write_u8_to_register(static_cast(Register::PAGE_ID), 0x00, ec); + if (ec) { + return false; + } + write_u8_to_register(static_cast(Register::UNIT_SEL), kUnitSelSi, ec); + if (ec) { + return false; + } + write_u8_to_register(static_cast(Register::SYS_TRIGGER), + use_external_crystal_ ? kSysTriggerExternalCrystal : 0x00, ec); + if (ec) { + return false; + } + std::this_thread::sleep_for(10ms); + + set_mode(mode_, ec); + if (ec) { + return false; + } + + uint8_t self_test = read_u8_from_register(static_cast(Register::ST_RESULT), ec); + if (ec) { + return false; + } + // ST_RESULT bits: 0 accel, 1 mag, 2 gyro, 3 MCU; 1 = passed. + if ((self_test & 0x0F) != 0x0F) { + logger_.warn("Self test incomplete: ST_RESULT=0x{:02x}", self_test); + } + + initialized_ = true; + logger_.info("Initialized in mode 0x{:02x}", static_cast(mode_)); + return true; +} + +bool Bno055::update(std::error_code &ec) { + std::array raw{}; + read_many_from_register(static_cast(Register::ACC_DATA_X_LSB), raw.data(), raw.size(), + ec); + if (ec) { + return false; + } + + auto parsed = parse(raw); + std::lock_guard lock(data_mutex_); + data_ = parsed; + return true; +} + +Bno055::Data Bno055::data() const { + std::lock_guard lock(data_mutex_); + return data_; +} + +void Bno055::read_system_status(uint8_t &status, uint8_t &error, std::error_code &ec) { + status = read_u8_from_register(static_cast(Register::SYS_STATUS), ec); + if (ec) { + return; + } + error = read_u8_from_register(static_cast(Register::SYS_ERR), ec); +} + +void Bno055::set_mode(OperatingMode mode, std::error_code &ec) { + write_u8_to_register(static_cast(Register::OPR_MODE), static_cast(mode), ec); + // Datasheet table 3-6: 19 ms switching into CONFIG, 7 ms switching out of it. + std::this_thread::sleep_for(mode == OperatingMode::CONFIG ? 25ms : 10ms); +} + +bool Bno055::wait_for_chip_id(std::error_code &ec) { + for (int attempt = 0; attempt < 20; ++attempt) { + ec.clear(); + uint8_t id = read_u8_from_register(static_cast(Register::CHIP_ID), ec); + if (!ec && id == CHIP_ID) { + return true; + } + std::this_thread::sleep_for(50ms); + } + if (!ec) { + ec = std::make_error_code(std::errc::no_such_device); + } + logger_.error("No BNO055 chip ID (0x{:02x}) at address 0x{:02x}", CHIP_ID, base_config_.address); + return false; +} + +Bno055::Data Bno055::parse(const std::array &raw) { + // Offsets are relative to ACC_DATA_X_LSB (0x08). + const uint8_t *b = raw.data(); + Data data; + data.accel_mps2 = to_vector(b + 0x00, kAccelLsbPerMps2); + data.mag_ut = to_vector(b + 0x06, kMagLsbPerUt); + data.gyro_radps = to_vector(b + 0x0C, kGyroLsbPerRadps); + data.euler.heading = to_i16(b + 0x12) / kEulerLsbPerRad; + data.euler.roll = to_i16(b + 0x14) / kEulerLsbPerRad; + data.euler.pitch = to_i16(b + 0x16) / kEulerLsbPerRad; + data.orientation.w = to_i16(b + 0x18) / kQuaternionLsbPerUnit; + data.orientation.x = to_i16(b + 0x1A) / kQuaternionLsbPerUnit; + data.orientation.y = to_i16(b + 0x1C) / kQuaternionLsbPerUnit; + data.orientation.z = to_i16(b + 0x1E) / kQuaternionLsbPerUnit; + data.linear_accel_mps2 = to_vector(b + 0x20, kAccelLsbPerMps2); + data.gravity_mps2 = to_vector(b + 0x26, kAccelLsbPerMps2); + data.temperature_c = static_cast(b[0x2C]); + const uint8_t calib = b[0x2D]; + data.calibration = {.system = static_cast((calib >> 6) & 0x03), + .gyro = static_cast((calib >> 4) & 0x03), + .accel = static_cast((calib >> 2) & 0x03), + .mag = static_cast(calib & 0x03)}; + return data; +} + +} // namespace espp diff --git a/doc/Doxyfile b/doc/Doxyfile index 386b4e65a6..97b1a6c0a1 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -92,6 +92,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/bldc_haptics/example/main/bldc_haptics_example.cpp \ $(PROJECT_PATH)/components/bm8563/example/main/bm8563_example.cpp \ $(PROJECT_PATH)/components/bmi270/example/main/bmi270_example.cpp \ + $(PROJECT_PATH)/components/bno055/example/main/bno055_example.cpp \ $(PROJECT_PATH)/components/bq27220/example/main/bq27220_example.cpp \ $(PROJECT_PATH)/components/button/example/main/button_example.cpp \ $(PROJECT_PATH)/components/byte90/example/main/byte90_example.cpp \ @@ -274,6 +275,7 @@ INPUT = \ $(PROJECT_PATH)/components/bm8563/include/bm8563.hpp \ $(PROJECT_PATH)/components/bmi270/include/bmi270.hpp \ $(PROJECT_PATH)/components/bmi270/include/bmi270_detail.hpp \ + $(PROJECT_PATH)/components/bno055/include/bno055.hpp \ $(PROJECT_PATH)/components/bq27220/include/bq27220.hpp \ $(PROJECT_PATH)/components/button/include/button.hpp \ $(PROJECT_PATH)/components/byte90/include/byte90.hpp \ diff --git a/doc/en/imu/bno055.rst b/doc/en/imu/bno055.rst new file mode 100644 index 0000000000..b9414cd348 --- /dev/null +++ b/doc/en/imu/bno055.rst @@ -0,0 +1,20 @@ +BNO055 9-Axis Absolute Orientation IMU +************************************** + +The `Bno055` component provides a driver for the Bosch BNO055 9-axis absolute +orientation sensor. The BNO055 runs sensor fusion on its own microcontroller +and reports orientation, linear acceleration, and gravity alongside the raw +accelerometer, gyroscope, and magnetometer data. + +.. ------------------------------- Example ------------------------------------- + +.. toctree:: + + bno055_example + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/bno055.inc diff --git a/doc/en/imu/bno055_example.md b/doc/en/imu/bno055_example.md new file mode 100644 index 0000000000..94c28b3d7a --- /dev/null +++ b/doc/en/imu/bno055_example.md @@ -0,0 +1,2 @@ +```{include} ../../../components/bno055/example/README.md +``` diff --git a/doc/en/imu/index.rst b/doc/en/imu/index.rst index 6058bdf49e..fc245c3475 100644 --- a/doc/en/imu/index.rst +++ b/doc/en/imu/index.rst @@ -6,6 +6,7 @@ IMU APIs adxl345 bmi270 + bno055 icm20948 icm42607 lsm6dso