Bno08xHandler.h

Go to the documentation of this file.

 
#ifndef COMPONENT_HANDLER_BNO08X_HANDLER_H_
#define COMPONENT_HANDLER_BNO08X_HANDLER_H_
 
#include <cstdint>
#include <memory>
#include <array>
#include <functional>
#include <type_traits>
#include <utility>
#include "core/hf-core-drivers/external/hf-bno08x-driver/inc/bno08x.hpp"
#include "core/hf-core-drivers/external/hf-bno08x-driver/inc/bno08x_comm_interface.hpp"
#include "base/BaseI2c.h"
#include "base/BaseSpi.h"
#include "base/BaseGpio.h"
#include "RtosMutex.h"
 
// ============================================================================
//  BNO08X ERROR CODES
// ============================================================================
 
enum class Bno08xError : uint8_t {
    SUCCESS = 0,                
    NOT_INITIALIZED,            
    INITIALIZATION_FAILED,      
    INVALID_PARAMETER,          
    COMMUNICATION_FAILED,       
    SENSOR_NOT_RESPONDING,      
    CALIBRATION_FAILED,         
    FIRMWARE_UPDATE_FAILED,     
    TIMEOUT,                    
    MUTEX_LOCK_FAILED,          
    INVALID_INTERFACE,          
    SENSOR_NOT_ENABLED,         
    DATA_NOT_AVAILABLE,         
    HARDWARE_ERROR,             
    UNSUPPORTED_OPERATION       
};
 
constexpr const char* Bno08xErrorToString(Bno08xError error) noexcept {
    switch (error) {
        case Bno08xError::SUCCESS:                return "Success";
        case Bno08xError::NOT_INITIALIZED:        return "Not initialized";
        case Bno08xError::INITIALIZATION_FAILED:   return "Initialization failed";
        case Bno08xError::INVALID_PARAMETER:       return "Invalid parameter";
        case Bno08xError::COMMUNICATION_FAILED:    return "Communication failed";
        case Bno08xError::SENSOR_NOT_RESPONDING:   return "Sensor not responding";
        case Bno08xError::CALIBRATION_FAILED:      return "Calibration failed";
        case Bno08xError::FIRMWARE_UPDATE_FAILED:  return "Firmware update failed";
        case Bno08xError::TIMEOUT:                 return "Timeout";
        case Bno08xError::MUTEX_LOCK_FAILED:       return "Mutex lock failed";
        case Bno08xError::INVALID_INTERFACE:       return "Invalid interface";
        case Bno08xError::SENSOR_NOT_ENABLED:      return "Sensor not enabled";
        case Bno08xError::DATA_NOT_AVAILABLE:      return "Data not available";
        case Bno08xError::HARDWARE_ERROR:          return "Hardware error";
        case Bno08xError::UNSUPPORTED_OPERATION:   return "Unsupported operation";
        default: return "Unknown error";
    }
}
 
// ============================================================================
//  CRTP COMMUNICATION ADAPTERS
// ============================================================================
 
class HalI2cBno08xComm : public bno08x::CommInterface<HalI2cBno08xComm> {
public:
    explicit HalI2cBno08xComm(BaseI2c& i2c,
                              BaseGpio* reset_gpio = nullptr,
                              BaseGpio* int_gpio = nullptr) noexcept
        : i2c_(i2c), reset_gpio_(reset_gpio), int_gpio_(int_gpio) {}
 
    // -- bno08x::CommInterface required methods --
 
    BNO085Interface GetInterfaceType() noexcept { return BNO085Interface::I2C; }
 
    bool Open() noexcept;
    void Close() noexcept;
    int  Write(const uint8_t* data, uint32_t length) noexcept;
    int  Read(uint8_t* data, uint32_t length) noexcept;
    bool DataAvailable() noexcept;
    void Delay(uint32_t ms) noexcept;
    uint32_t GetTimeUs() noexcept;
 
    bool GpioSet(bno08x::CtrlPin pin, bno08x::GpioSignal signal) noexcept;
 
private:
    BaseI2c& i2c_;
    BaseGpio* reset_gpio_;
    BaseGpio* int_gpio_;
};
 
class HalSpiBno08xComm : public bno08x::CommInterface<HalSpiBno08xComm> {
public:
    explicit HalSpiBno08xComm(BaseSpi& spi,
                              BaseGpio* reset_gpio = nullptr,
                              BaseGpio* int_gpio = nullptr,
                              BaseGpio* wake_gpio = nullptr) noexcept
        : spi_(spi), reset_gpio_(reset_gpio), int_gpio_(int_gpio), wake_gpio_(wake_gpio) {}
 
    // -- bno08x::CommInterface required methods --
 
    BNO085Interface GetInterfaceType() noexcept { return BNO085Interface::SPI; }
 
    bool Open() noexcept;
    void Close() noexcept;
    int  Write(const uint8_t* data, uint32_t length) noexcept;
    int  Read(uint8_t* data, uint32_t length) noexcept;
    bool DataAvailable() noexcept;
    void Delay(uint32_t ms) noexcept;
    uint32_t GetTimeUs() noexcept;
 
    bool GpioSet(bno08x::CtrlPin pin, bno08x::GpioSignal signal) noexcept;
 
private:
    BaseSpi& spi_;
    BaseGpio* reset_gpio_;
    BaseGpio* int_gpio_;
    BaseGpio* wake_gpio_;
};
 
// ============================================================================
//  TYPE-ERASED DRIVER INTERFACE (internal)
// ============================================================================
 
class IBno08xDriverOps {
public:
    virtual ~IBno08xDriverOps() noexcept = default;
 
    virtual bool Begin() noexcept = 0;
    virtual void Update() noexcept = 0;
    virtual bool EnableSensor(BNO085Sensor sensor, uint32_t interval_ms,
                              float sensitivity) noexcept = 0;
    virtual bool DisableSensor(BNO085Sensor sensor) noexcept = 0;
    virtual void SetCallback(SensorCallback cb) noexcept = 0;
    virtual bool HasNewData(BNO085Sensor sensor) const noexcept = 0;
    virtual SensorEvent GetLatest(BNO085Sensor sensor) const noexcept = 0;
    virtual int  GetLastError() const noexcept = 0;
    virtual bool HardwareReset(uint32_t lowMs) noexcept = 0;
    virtual bool SetBootPin(bool state) noexcept = 0;
    virtual bool SetWakePin(bool state) noexcept = 0;
    virtual bool SelectInterface(BNO085Interface iface) noexcept = 0;
    virtual BNO085Interface GetInterfaceType() noexcept = 0;
};
 
template <typename CommType>
class Bno08xDriverImpl : public IBno08xDriverOps {
public:
    explicit Bno08xDriverImpl(CommType&& comm) noexcept
        : comm_(std::move(comm)), driver_(comm_) {}
 
    bool Begin() noexcept override { return driver_.Begin(); }
    void Update() noexcept override { driver_.Update(); }
 
    bool EnableSensor(BNO085Sensor sensor, uint32_t interval_ms,
                      float sensitivity) noexcept override {
        return driver_.EnableSensor(sensor, interval_ms, sensitivity);
    }
 
    bool DisableSensor(BNO085Sensor sensor) noexcept override {
        return driver_.DisableSensor(sensor);
    }
 
    void SetCallback(SensorCallback cb) noexcept override {
        driver_.SetCallback(std::move(cb));
    }
 
    bool HasNewData(BNO085Sensor sensor) const noexcept override {
        return driver_.HasNewData(sensor);
    }
 
    SensorEvent GetLatest(BNO085Sensor sensor) const noexcept override {
        return driver_.GetLatest(sensor);
    }
 
    int GetLastError() const noexcept override {
        return driver_.GetLastError();
    }
 
    bool HardwareReset(uint32_t lowMs) noexcept override {
        return driver_.HardwareReset(lowMs);
    }
 
    bool SetBootPin(bool state) noexcept override {
        return driver_.SetBootPin(state);
    }
 
    bool SetWakePin(bool state) noexcept override {
        return driver_.SetWakePin(state);
    }
 
    bool SelectInterface(BNO085Interface iface) noexcept override {
        return driver_.SelectInterface(iface);
    }
 
    BNO085Interface GetInterfaceType() noexcept override {
        return comm_.GetInterfaceType();
    }
 
private:
    CommType comm_;                  
    mutable BNO085<CommType> driver_;  
};
 
// ============================================================================
//  SENSOR DATA STRUCTURES
// ============================================================================
 
struct Bno08xVector3 {
    float x{0};                  
    float y{0};                  
    float z{0};                  
    uint8_t accuracy{0};         
    uint64_t timestamp_us{0};    
    bool valid{false};           
};
 
struct Bno08xQuaternion {
    float w{1};                  
    float x{0};                  
    float y{0};                  
    float z{0};                  
    uint8_t accuracy{0};         
    uint64_t timestamp_us{0};    
    bool valid{false};           
};
 
struct Bno08xEulerAngles {
    float roll{0};               
    float pitch{0};              
    float yaw{0};                
    uint8_t accuracy{0};         
    uint64_t timestamp_us{0};    
    bool valid{false};           
};
 
struct Bno08xImuData {
    Bno08xVector3 acceleration;        
    Bno08xVector3 gyroscope;           
    Bno08xVector3 magnetometer;        
    Bno08xVector3 linear_acceleration; 
    Bno08xVector3 gravity;             
    Bno08xQuaternion rotation;         
    Bno08xEulerAngles euler;           
    uint64_t timestamp_us{0};          
    bool valid{false};                 
};
 
struct Bno08xActivityData {
    bool tap_detected{false};        
    bool double_tap{false};          
    uint8_t tap_direction{0};        
    uint32_t step_count{0};          
    bool step_detected{false};       
    bool shake_detected{false};      
    bool pickup_detected{false};     
    bool significant_motion{false};  
    uint8_t stability_class{0};      
    uint64_t timestamp_us{0};        
    bool valid{false};               
};
 
struct Bno08xCalibrationStatus {
    uint8_t accelerometer_accuracy{0};  
    uint8_t gyroscope_accuracy{0};      
    uint8_t magnetometer_accuracy{0};   
    uint8_t rotation_accuracy{0};       
    bool fully_calibrated{false};       
};
 
struct Bno08xConfig {
    // Sensor enable flags
    bool enable_accelerometer{true};       
    bool enable_gyroscope{true};           
    bool enable_magnetometer{true};        
    bool enable_rotation_vector{true};    
    bool enable_linear_acceleration{false}; 
    bool enable_gravity{false};            
    bool enable_game_rotation{false};      
 
    // Activity detection (interval 0 = on-change)
    bool enable_tap_detector{false};      
    bool enable_step_counter{false};       
    bool enable_step_detector{false};      
    bool enable_shake_detector{false};     
    bool enable_pickup_detector{false};    
    bool enable_significant_motion{false}; 
    bool enable_stability_classifier{false}; 
 
    // Sensor intervals (milliseconds, 0 = on-change for event sensors)
    uint32_t accelerometer_interval_ms{50};     
    uint32_t gyroscope_interval_ms{50};         
    uint32_t magnetometer_interval_ms{100};     
    uint32_t rotation_interval_ms{50};          
    uint32_t linear_accel_interval_ms{50};      
    uint32_t gravity_interval_ms{100};          
    uint32_t game_rotation_interval_ms{50};     
};
 
// ============================================================================
//  BNO08X HANDLER CLASS
// ============================================================================
 
class Bno08xHandler {
public:
    // ========================================================================
    //  CONSTRUCTION AND LIFECYCLE
    // ========================================================================
 
    explicit Bno08xHandler(BaseI2c& i2c_device,
                           const Bno08xConfig& config = Bno08xConfig{},
                           BaseGpio* reset_gpio = nullptr,
                           BaseGpio* int_gpio = nullptr) noexcept;
 
    explicit Bno08xHandler(BaseSpi& spi_device,
                           const Bno08xConfig& config = Bno08xConfig{},
                           BaseGpio* reset_gpio = nullptr,
                           BaseGpio* int_gpio = nullptr,
                           BaseGpio* wake_gpio = nullptr) noexcept;
 
    ~Bno08xHandler() noexcept = default;
 
    // Non-copyable
    Bno08xHandler(const Bno08xHandler&) = delete;
    Bno08xHandler& operator=(const Bno08xHandler&) = delete;
 
    // Non-movable
    Bno08xHandler(Bno08xHandler&&) = delete;
    Bno08xHandler& operator=(Bno08xHandler&&) = delete;
 
    // ========================================================================
    //  INITIALIZATION AND STATUS
    // ========================================================================
 
    Bno08xError Initialize() noexcept;
 
    bool EnsureInitialized() noexcept;
 
    Bno08xError Deinitialize() noexcept;
 
    bool IsInitialized() const noexcept;
 
    // ========================================================================
    //  SERVICE LOOP
    // ========================================================================
 
    Bno08xError Update() noexcept;
 
    // ========================================================================
    //  CALLBACK MANAGEMENT
    // ========================================================================
 
    void SetSensorCallback(SensorCallback callback) noexcept;
 
    void ClearSensorCallback() noexcept;
 
    // ========================================================================
    //  UTILITY METHODS
    // ========================================================================
 
    static void QuaternionToEuler(const Bno08xQuaternion& quaternion,
                                  Bno08xEulerAngles& euler_angles) noexcept;
 
    BNO085Interface GetInterfaceType() const noexcept;
 
    IBno08xDriverOps* GetSensor() noexcept;
 
    const IBno08xDriverOps* GetSensor() const noexcept;
 
    IBno08xDriverOps* GetDriver() noexcept { return GetSensor(); }
    const IBno08xDriverOps* GetDriver() const noexcept { return GetSensor(); }
 
    template <typename Fn>
    auto visitDriver(Fn&& fn) noexcept -> decltype(fn(std::declval<IBno08xDriverOps&>())) {
        using ReturnType = decltype(fn(std::declval<IBno08xDriverOps&>()));
        MutexLockGuard lock(handler_mutex_);
        if (!lock.IsLocked() || !ensureInitializedLocked() || !driver_ops_) {
            if constexpr (std::is_void_v<ReturnType>) {
                return;
            } else {
                return ReturnType{};
            }
        }
        return fn(*driver_ops_);
    }
 
    Bno08xError GetLastError() const noexcept;
 
    int GetLastDriverError() const noexcept;
 
    const char* GetDescription() const noexcept;
 
    static Bno08xConfig GetDefaultConfig() noexcept { return Bno08xConfig{}; }
 
    void DumpDiagnostics() const noexcept;
 
private:
    // ========================================================================
    //  PRIVATE MEMBERS
    // ========================================================================
 
    std::unique_ptr<IBno08xDriverOps> driver_ops_; 
    Bno08xConfig config_;                          
    mutable RtosMutex handler_mutex_;              
    bool initialized_{false};                      
    mutable Bno08xError last_error_{Bno08xError::SUCCESS}; 
    BNO085Interface interface_type_;               
    SensorCallback user_callback_;                 
    char description_[64]{};                       
 
    // ========================================================================
    //  PRIVATE HELPERS
    // ========================================================================
 
    bool ensureInitializedLocked() noexcept;
 
    bool applyConfigLocked() noexcept;
 
    static Bno08xError mapDriverError(int sh2_error) noexcept;
};
 
// ============================================================================
//  FACTORY METHODS
// ============================================================================
 
inline std::unique_ptr<Bno08xHandler> CreateBno08xHandlerI2c(
    BaseI2c& i2c_device,
    const Bno08xConfig& config = Bno08xConfig{},
    BaseGpio* reset_gpio = nullptr,
    BaseGpio* int_gpio = nullptr) noexcept {
    return std::make_unique<Bno08xHandler>(i2c_device, config, reset_gpio, int_gpio);
}
 
inline std::unique_ptr<Bno08xHandler> CreateBno08xHandlerSpi(
    BaseSpi& spi_device,
    const Bno08xConfig& config = Bno08xConfig{},
    BaseGpio* reset_gpio = nullptr,
    BaseGpio* int_gpio = nullptr,
    BaseGpio* wake_gpio = nullptr) noexcept {
    return std::make_unique<Bno08xHandler>(spi_device, config, reset_gpio, int_gpio, wake_gpio);
}
 
#endif // COMPONENT_HANDLER_BNO08X_HANDLER_H_