bno08x.hpp

Go to the documentation of this file.

#pragma once
#include "bno08x_comm_interface.hpp"
#include "bno08x_version.h"
#include "dfu/HcBin.h"
#include <array>
#include <atomic>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <functional>
 
extern "C" {
#include "sh2/sh2.h"
#include "sh2/sh2_SensorValue.h"
#include "sh2/sh2_err.h"
}
 
inline constexpr int BNO08X_ERR_SH2_SESSION_IN_USE = -100;
 
namespace bno08x::detail {
inline std::atomic<const void*>& Sh2SessionOwner() noexcept {
  static std::atomic<const void*> owner{nullptr};
  return owner;
}
} // namespace bno08x::detail
 
extern const HcBin_t firmware;
 
// ============================================================================
// ============================================================================
 
enum class BNO085Sensor : uint8_t {
  Accelerometer = 0x01,              
  Gyroscope = 0x02,                  
  Magnetometer = 0x03,               
  LinearAcceleration = 0x04,         
  RotationVector = 0x05,             
  Gravity = 0x06,                    
  GyroUncalibrated = 0x07,           
  GameRotationVector = 0x08,         
  GeomagneticRotationVector = 0x09,  
  Pressure = 0x0A,                   
  AmbientLight = 0x0B,               
  Humidity = 0x0C,                   
  Proximity = 0x0D,                  
  Temperature = 0x0E,                
  MagneticFieldUncalibrated = 0x0F,  
  TapDetector = 0x10,                
  StepCounter = 0x11,                
  SignificantMotion = 0x12,          
  StabilityClassifier = 0x13,        
  RawAccelerometer = 0x14,           
  RawGyroscope = 0x15,               
  RawMagnetometer = 0x16,            
  StepDetector = 0x18,               
  ShakeDetector = 0x19,              
  FlipDetector = 0x1A,               
  PickupDetector = 0x1B,             
  StabilityDetector = 0x1C,          
  PersonalActivityClassifier = 0x1E, 
  SleepDetector = 0x1F,              
  TiltDetector = 0x20,               
  PocketDetector = 0x21,             
  CircleDetector = 0x22,             
  HeartRateMonitor = 0x23,           
  ARVRStabilizedRV = 0x28,           
  ARVRStabilizedGameRV = 0x29,       
  GyroIntegratedRV = 0x2A,           
  IZroMotionRequest = 0x2B,          
  RawOpticalFlow = 0x2C,             
  DeadReckoningPose = 0x2D,          
  WheelEncoder = 0x2E                
};
 
struct Vector3 {
  float x{0};          
  float y{0};          
  float z{0};          
  uint8_t accuracy{0}; 
};
 
struct Quaternion {
  float w{1};          
  float x{0};          
  float y{0};          
  float z{0};          
  uint8_t accuracy{0}; 
};
 
struct TapEvent {
  bool doubleTap{false}; 
  uint8_t direction{0};  
};
 
struct RawVector3 {
  int16_t x{0};
  int16_t y{0};
  int16_t z{0};
  int16_t temperature{0};   
  uint32_t sensorTimeUs{0}; 
};
 
struct ActivityClassifierEvent {
  uint8_t page{0};
  bool lastPage{false};
  uint8_t mostLikelyState{0};
  std::array<uint8_t, 10> confidence{};
};
 
struct RawOpticalFlowEvent {
  uint32_t timestamp{0};
  int16_t dt{0};
  int16_t dx{0};
  int16_t dy{0};
  int16_t iq{0};
  uint8_t resX{0};
  uint8_t resY{0};
  uint8_t shutter{0};
  uint8_t frameMax{0};
  uint8_t frameAvg{0};
  uint8_t frameMin{0};
  uint8_t laserOn{0};
};
 
struct DeadReckoningPoseEvent {
  uint32_t timestamp{0};
  Vector3 linearPosition{};
  Quaternion rotation{};
  Vector3 linearVelocity{};
  Vector3 angularVelocity{};
};
 
struct WheelEncoderEvent {
  uint32_t timestamp{0};
  uint8_t wheelIndex{0};
  uint8_t dataType{0};
  uint16_t data{0};
};
 
struct SensorEvent {
  BNO085Sensor sensor{BNO085Sensor::Accelerometer}; 
  uint64_t timestamp{0};                            
  Vector3 vector{};                                 
  Vector3 bias{};                                   
  Vector3 angularVelocity{};                        
  Quaternion rotation{};                            
  float scalar{0};                    
  RawVector3 raw{};                   
  uint32_t latencyUs{0};              
  uint32_t stepCount{0};              
  uint16_t eventFlags{0};             
  uint8_t classification{0};          
  uint8_t sleepState{0};              
  TapEvent tap{};                     
  ActivityClassifierEvent activity{}; 
  uint8_t motionIntent{0};            
  uint8_t motionRequest{0};           
  RawOpticalFlowEvent rawOpticalFlow{};
  DeadReckoningPoseEvent deadReckoningPose{};
  WheelEncoderEvent wheelEncoder{};
  bool detected{false}; 
};
 
// ---- RVC Mode Data Types ---------------------------------------------------
 
struct RvcSensorEvent {
  uint8_t index;         
  int16_t yaw;           
  int16_t pitch;         
  int16_t roll;          
  int16_t acc_x;         
  int16_t acc_y;         
  int16_t acc_z;         
  uint8_t mi;            
  uint8_t mr;            
  uint64_t timestamp_uS; 
};
 
struct RvcSensorValue {
  uint8_t index;         
  float yaw_deg;         
  float pitch_deg;       
  float roll_deg;        
  float acc_x_g;         
  float acc_y_g;         
  float acc_z_g;         
  uint8_t mi;            
  uint8_t mr;            
  uint64_t timestamp_uS; 
};
 
// ---- Callback Types --------------------------------------------------------
 
using SensorCallback = std::function<void(const SensorEvent&)>;
 
using RvcCallback = std::function<void(const RvcSensorValue&)>;
 
enum class BNO085DriverState : uint8_t { Closed = 0, Sh2Active, RvcActive, DfuInProgress };
 
struct DfuProgress {
  uint32_t bytesSent{0};  
  uint32_t totalBytes{0}; 
};
 
using DfuProgressCallback = std::function<void(const DfuProgress&)>;
 
struct DfuOptions {
  bool requireFormatMatch{true};           
  bool requirePartNumber{true};            
  const char* requiredFormat{"BNO_V1"};    
  const char* requiredPartNumber{nullptr}; 
  uint8_t packetLenOverride{0};            
  DfuProgressCallback progress{};          
};
 
struct DfuMemoryImage {
  const uint8_t* data{nullptr};        
  uint32_t length{0};                  
  const char* format{"BNO_V1"};        
  const char* partNumber{"1000-3608"}; 
  uint32_t preferredPacketLen{0};      
};
 
 
// ============================================================================
// ============================================================================
 
template <typename CommType>
class BNO085 {
public:
  explicit BNO085(CommType& comm) noexcept : io_(comm) {
    prepareHalWrapper();
  }
 
  ~BNO085() noexcept {
    Close();
    CloseRvc();
  }
 
  // --------------------------------------------------------------------------
 
  bool Begin() noexcept;
 
  static bool Sh2SessionInUse() noexcept {
    return bno08x::detail::Sh2SessionOwner().load() != nullptr;
  }
 
  void Close() noexcept;
 
  bool EnableSensor(BNO085Sensor sensor, uint32_t interval_ms, float sensitivity = 0.0f) noexcept;
 
  bool DisableSensor(BNO085Sensor sensor) noexcept;
 
  void SetCallback(SensorCallback cb) noexcept;
 
  bool HasNewData(BNO085Sensor sensor) const;
 
  SensorEvent GetLatest(BNO085Sensor sensor) noexcept;
 
  void Update() noexcept;
 
 
  // --------------------------------------------------------------------------
 
  void SetRvcCallback(RvcCallback cb) noexcept;
 
  bool BeginRvc() noexcept;
 
  void ServiceRvc() noexcept;
 
  void CloseRvc() noexcept;
 
 
  // --------------------------------------------------------------------------
 
  int GetLastError() const {
    return last_error_;
  }
 
  BNO085DriverState GetState() const noexcept {
    return state_;
  }
 
  bool HardwareReset(uint32_t lowMs = 2) noexcept;
 
  static constexpr uint32_t RESET_STARTUP_MS = 90 + 4;
 
  static constexpr uint8_t I2C_ADDRESS_APP = 0x4A;
  static constexpr uint8_t I2C_ADDRESS_BOOTLOADER = 0x28;
 
  static constexpr uint8_t BootloaderI2cAddress(uint8_t appAddress) noexcept {
    const auto sa0 = static_cast<uint8_t>(appAddress & 0x01U);
    const auto base = static_cast<uint8_t>(appAddress & 0xFEU);
    if (base == I2C_ADDRESS_APP || base == I2C_ADDRESS_BOOTLOADER) {
      return static_cast<uint8_t>(I2C_ADDRESS_BOOTLOADER | sa0);
    }
    return 0;
  }
 
  bool SetBootPin(bool state) noexcept;
 
  bool SetWakePin(bool state) noexcept;
 
  bool SelectInterface(BNO085Interface iface) noexcept;
 
  int Dfu(const HcBin_t& fw = firmware) noexcept;
 
  int DfuWithOptions(const HcBin_t& fw, const DfuOptions& options) noexcept;
 
  int DfuFromMemory(const DfuMemoryImage& image, const DfuOptions& options = {}) noexcept;
 
  int DfuFromMemory(const uint8_t* data, uint32_t len, const char* partNumber = "1000-3608",
                    const DfuOptions& options = {}) noexcept;
 
  bool EnterBootloader(uint32_t resetLowMs = 10, uint32_t settleMs = 50) noexcept;
 
  bool ExitBootloaderAndReboot(uint32_t resetLowMs = 2, uint32_t settleMs = 100) noexcept;
 
  int RunDfuFromMemory(const DfuMemoryImage& image, const DfuOptions& options = {},
                       uint32_t enterResetLowMs = 10, uint32_t enterSettleMs = 50,
                       uint32_t exitResetLowMs = 2, uint32_t exitSettleMs = 100) noexcept;
 
 
  // ===========================================================================
  // Driver Version
  // ===========================================================================
 
  static constexpr const char* GetDriverVersion() noexcept {
    return HF_BNO08X_VERSION_STRING;
  }
 
  static constexpr uint8_t GetDriverVersionMajor() noexcept {
    return HF_BNO08X_VERSION_MAJOR;
  }
 
  static constexpr uint8_t GetDriverVersionMinor() noexcept {
    return HF_BNO08X_VERSION_MINOR;
  }
 
  static constexpr uint8_t GetDriverVersionPatch() noexcept {
    return HF_BNO08X_VERSION_PATCH;
  }
 
private:
  void prepareHalWrapper() noexcept;
 
  bool acquireSh2Session() noexcept;
  void releaseSh2Session() noexcept;
 
  bool setPin(bno08x::CtrlPin pin, bool active) noexcept;
 
  SensorEvent toSensorEvent(BNO085Sensor sensor, const sh2_SensorValue_t& val) const noexcept;
 
  // --------------------------------------------------------------------------
 
  // Wrapper pairing the C HAL struct with the typed CommInterface pointer.
  struct CommHal {
    sh2_Hal_t* asHal() {
      return &hal;
    }
    sh2_Hal_t hal;           
    CommType* comm{nullptr}; 
  } halWrapper_{};
 
  static int halOpen(sh2_Hal_t* self);                                          
  static void halClose(sh2_Hal_t* self);                                        
  static int halRead(sh2_Hal_t* self, uint8_t* buf, unsigned len, uint32_t* t); 
  static int halWrite(sh2_Hal_t* self, uint8_t* buf, unsigned len);             
  static uint32_t halGetTimeUs(sh2_Hal_t* self);                                
 
 
  // --------------------------------------------------------------------------
 
  static void sensorCallback(void* cookie, sh2_SensorEvent_t* event); 
  static void asyncCallback(void* cookie, sh2_AsyncEvent_t* event);   
 
 
  // --------------------------------------------------------------------------
 
  void handleSensorEvent(const sh2_SensorEvent_t* event) noexcept;
  void handleAsyncEvent(const sh2_AsyncEvent_t* event) noexcept;
  bool configure(BNO085Sensor sensor, uint32_t interval_us, float sensitivity,
                 uint32_t batch_us = 0) noexcept;
 
 
  // --------------------------------------------------------------------------
 
  static constexpr uint8_t RVC_FRAME_LEN_ = 19; 
 
  void rvcProcessByte(uint8_t c) noexcept;
 
  static void decodeRvc(RvcSensorValue* out, const RvcSensorEvent* in) noexcept;
 
  uint8_t rvc_frame_[RVC_FRAME_LEN_]{}; 
  uint8_t rvc_frame_len_{0};            
 
 
  // --------------------------------------------------------------------------
 
  static void dfuWrite32be(uint8_t* buf, uint32_t value) noexcept;           
  static bool dfuIsKnownPartNumber(const char* part) noexcept;               
  static void dfuAppendCrc(uint8_t* packet, uint8_t len) noexcept;           
  int dfuSend(uint8_t* p_data, uint32_t len) noexcept;                       
  int dfuSendAppSize(uint8_t* dfu_buff, uint32_t app_size) noexcept;         
  int dfuSendPktSize(uint8_t* dfu_buff, uint8_t packet_len) noexcept;        
  int dfuSendPkt(uint8_t* dfu_buff, uint8_t* p_data, uint32_t len) noexcept; 
  int dfuSelectBootloaderAddress(uint8_t& appAddress, bool& switched) noexcept; 
  bool dfuRestoreAppAddress(uint8_t appAddress) noexcept;                       
 
 
  // --------------------------------------------------------------------------
 
  CommType& io_;              
  SensorCallback callback_{}; 
  RvcCallback rvc_cb_{};      
  int last_error_{0};         
  BNO085DriverState state_{BNO085DriverState::Closed}; 
 
  static constexpr std::size_t SENSOR_CACHE_SIZE_ = 0x2F;      
  std::array<sh2_SensorValue_t, SENSOR_CACHE_SIZE_> latest_{}; 
  std::array<bool, SENSOR_CACHE_SIZE_> new_flag_{};            
  std::array<uint32_t, SENSOR_CACHE_SIZE_>
      last_interval_{}; 
  std::array<float, SENSOR_CACHE_SIZE_> last_sensitivity_{}; 
 
};
 
 
// =============================================================================
// Free-function version accessor (no namespace — class is at global scope)
// =============================================================================
 
inline const char* GetBNO08xDriverVersion() noexcept {
  return HF_BNO08X_VERSION_STRING;
}
 
// ============================================================================
// Template Implementation
// ============================================================================
// The template method bodies live in src/bno08x.ipp and are included here so
// that the compiler can instantiate them for any CommType. src/ is on the
// include path in the build tree; an installed package puts bno08x.ipp next
// to this header.
#define BNO085_HEADER_INCLUDED
// NOLINTNEXTLINE(bugprone-suspicious-include) - Intentional: template implementation file
#include "bno08x.ipp"
#undef BNO085_HEADER_INCLUDED