ImuManager.h

Go to the documentation of this file.

 
#pragma once
 
#include <array>
#include <optional>
#include "RtosMutex.h"
#include "SignalSemaphore.h"
#include "handlers/bno08x/Bno08xHandler.h"
 
// Forward declarations (types defined in other managers)
class CommChannelsManager;
enum class SpiDeviceId : uint8_t;
class GpioManager;
class BaseGpio;
class BaseI2c;
class BaseSpi;
 
using ImuInterruptCallback = void (*)(void* user_data);
 
//==============================================================================
// IMU ERROR CODES
//==============================================================================
 
enum class ImuError : uint8_t {
    SUCCESS = 0,                   
    NOT_INITIALIZED,               
    INITIALIZATION_FAILED,         
    DEVICE_ALREADY_EXISTS,         
    DEVICE_NOT_FOUND,              
    INVALID_DEVICE_INDEX,          
    CANNOT_DELETE_ONBOARD,         
    INVALID_I2C_ADDRESS,           
    DEPENDENCY_NOT_READY,          
    COMMUNICATION_FAILED,          
    I2C_DEVICE_CREATION_FAILED,    
    HANDLER_CREATION_FAILED,       
    INTERRUPT_ERROR,               
    ALREADY_DEINITIALIZED,         
    MUTEX_LOCK_FAILED              
};
 
constexpr const char* ImuErrorToString(ImuError error) noexcept {
    switch (error) {
        case ImuError::SUCCESS:                    return "Success";
        case ImuError::NOT_INITIALIZED:            return "Not initialized";
        case ImuError::INITIALIZATION_FAILED:      return "Initialization failed";
        case ImuError::DEVICE_ALREADY_EXISTS:      return "Device already exists";
        case ImuError::DEVICE_NOT_FOUND:           return "Device not found";
        case ImuError::INVALID_DEVICE_INDEX:       return "Invalid device index";
        case ImuError::CANNOT_DELETE_ONBOARD:      return "Cannot delete onboard device";
        case ImuError::INVALID_I2C_ADDRESS:        return "Invalid I2C address";
        case ImuError::DEPENDENCY_NOT_READY:       return "Dependency not ready";
        case ImuError::COMMUNICATION_FAILED:       return "Communication failed";
        case ImuError::I2C_DEVICE_CREATION_FAILED: return "I2C device creation failed";
        case ImuError::HANDLER_CREATION_FAILED:    return "Handler creation failed";
        case ImuError::INTERRUPT_ERROR:            return "Interrupt configuration error";
        case ImuError::ALREADY_DEINITIALIZED:      return "Already deinitialized";
        case ImuError::MUTEX_LOCK_FAILED:          return "Mutex lock failed";
        default:                                   return "Unknown error";
    }
}
 
//==============================================================================
// IMU DIAGNOSTICS
//==============================================================================
 
struct ImuSystemDiagnostics {
    bool system_healthy;               
    bool system_initialized;           
    uint8_t active_device_count;       
    uint8_t initialized_device_count;  
    ImuError last_error;               
 
    struct DeviceSnapshot {
        bool active;                   
        bool initialized;              
        uint32_t error_count;          
    };
    static constexpr uint8_t kMaxDevices = 4;
    DeviceSnapshot devices[kMaxDevices]{}; 
 
    uint32_t total_interrupt_count;    
    bool interrupt_configured;         
    bool interrupt_enabled;            
};
 
class ImuManager {
public:
    static constexpr uint8_t MAX_IMU_DEVICES = 4;           
    static constexpr uint8_t ONBOARD_IMU_INDEX = 0;         
    static constexpr uint8_t EXTERNAL_IMU_1_INDEX = 1;      
    static constexpr uint8_t EXTERNAL_IMU_2_INDEX = 2;      
    static constexpr uint8_t EXTERNAL_IMU_3_INDEX = 3;      
 
public:
    static ImuManager& GetInstance() noexcept;
 
    //**************************************************************************//
    //**                  DEVICE MANAGEMENT METHODS                           **//
    //**************************************************************************//
 
    bool EnsureInitialized() noexcept;
 
    bool IsInitialized() const noexcept;
 
    [[nodiscard]] ImuError GetLastError() const noexcept { return last_error_.load(std::memory_order_acquire); }
 
    [[nodiscard]] ImuError GetSystemDiagnostics(ImuSystemDiagnostics& diagnostics) const noexcept;
 
    bool Deinitialize() noexcept;
 
    //**************************************************************************//
    //**                  HANDLER AND DRIVER MANAGEMENT                       **//
    //**************************************************************************//
 
    Bno08xHandler* GetBno08xHandler(uint8_t deviceIndex = ONBOARD_IMU_INDEX) noexcept;
 
    template <typename Fn>
    bool VisitSensor(uint8_t deviceIndex, Fn&& fn) noexcept {
        MutexLockGuard lock(manager_mutex_);
        if (deviceIndex >= MAX_IMU_DEVICES || !device_active_[deviceIndex] ||
            !device_initialized_[deviceIndex] || !bno08x_handlers_[deviceIndex]) {
            return false;
        }
        bool ran = false;
        bno08x_handlers_[deviceIndex]->visitDriver([&](IBno08xDriverOps& driver) {
            ran = true;
            fn(driver);
        });
        return ran;
    }
 
    //**************************************************************************//
    //**                  DEVICES MANAGEMENT METHODS                           **//
    //**************************************************************************//
 
    [[nodiscard]] ImuError CreateExternalBno08xDevice(uint8_t deviceIndex, 
                                   uint8_t i2c_address,
                                   uint32_t i2c_speed_hz = 400000,
                                   const Bno08xConfig& config = Bno08xHandler::GetDefaultConfig()) noexcept;
 
 
 
    [[nodiscard]] ImuError CreateExternalBno08xDevice(uint8_t deviceIndex, 
                                   SpiDeviceId spiDeviceId,
                                   const Bno08xConfig& config = Bno08xHandler::GetDefaultConfig()) noexcept;
 
    [[nodiscard]] ImuError CreateExternalBno08xDevice(uint8_t deviceIndex, 
                                   BaseI2c& i2c_interface,
                                   const Bno08xConfig& config = Bno08xHandler::GetDefaultConfig()) noexcept;
 
    [[nodiscard]] ImuError CreateExternalBno08xDevice(uint8_t deviceIndex, 
                                   BaseSpi& spi_interface,
                                   const Bno08xConfig& config = Bno08xHandler::GetDefaultConfig()) noexcept;
 
    [[nodiscard]] ImuError DeleteExternalDevice(uint8_t deviceIndex) noexcept;
 
    uint8_t GetDeviceCount() const noexcept;
 
    bool IsDeviceValid(uint8_t deviceIndex) const noexcept;
 
    bool IsExternalSlotAvailable(uint8_t deviceIndex) const noexcept;
 
    void GetActiveDeviceIndices(std::array<uint8_t, MAX_IMU_DEVICES>& out, size_t& count) const noexcept;
 
    size_t InitializeAllDevices(std::array<bool, MAX_IMU_DEVICES>& results) noexcept;
 
    void GetInitializationStatus(std::array<bool, MAX_IMU_DEVICES>& status) const noexcept;
 
    //**************************************************************************//
    //**                  DEVICE INFORMATION METHODS                          **//
    //**************************************************************************//
 
    void GetAvailableDevices(std::array<const char*, MAX_IMU_DEVICES>& out, size_t& count) const noexcept;
 
    [[nodiscard]] const char* GetDeviceType(uint8_t deviceIndex) const noexcept;
 
    //**************************************************************************//
    //**                  INTERRUPT SUPPORT METHODS                           **//
    //**************************************************************************//
 
    bool ConfigureInterrupt(uint8_t deviceIndex, ImuInterruptCallback callback = nullptr,
                            void* user_data = nullptr) noexcept;
 
    bool EnableInterrupt(uint8_t deviceIndex) noexcept;
 
    bool DisableInterrupt(uint8_t deviceIndex) noexcept;
 
    bool IsInterruptEnabled(uint8_t deviceIndex) const noexcept;
 
    bool WaitForInterrupt(uint8_t deviceIndex, uint32_t timeout_ms = 0) noexcept;
 
    uint32_t GetInterruptCount(uint8_t deviceIndex) const noexcept;
    
    void DumpStatistics() const noexcept;
 
    // Delete copy/move constructors and assignment operators
    ImuManager(const ImuManager&) = delete;
    ImuManager& operator=(const ImuManager&) = delete;
    ImuManager(ImuManager&&) = delete;
    ImuManager& operator=(ImuManager&&) = delete;
 
private:
    ImuManager() noexcept;
 
    ~ImuManager() noexcept;
 
    bool Initialize() noexcept;
 
    bool InitializeOnboardBno08xDevice() noexcept;
 
    bool InitializeInterruptGpio() noexcept;
 
    bool IsExternalDeviceIndex(uint8_t deviceIndex) const noexcept;
 
    void UpdateLastError(ImuError error, int deviceIndex = -1) noexcept;
 
    static void GpioInterruptHandler(BaseGpio* gpio, hf_gpio_interrupt_trigger_t trigger, void* user_data) noexcept;
 
    // ===============================
    // SYSTEM STATE
    // ===============================
 
    std::atomic<bool> initialized_{false};
 
    std::atomic<ImuError> last_error_{ImuError::SUCCESS};
 
    std::array<std::atomic<uint32_t>, MAX_IMU_DEVICES> device_error_counts_{};
 
    mutable RtosMutex manager_mutex_;
 
    // ===============================
    // DEVICE STORAGE
    // ===============================
 
    std::array<std::optional<Bno08xHandler>, MAX_IMU_DEVICES> bno08x_handlers_;
 
    std::array<bool, MAX_IMU_DEVICES> device_initialized_;
 
    std::array<bool, MAX_IMU_DEVICES> device_active_;
 
    bool onboard_device_created_ = false;
 
    std::array<int, MAX_IMU_DEVICES> i2c_device_indices_;
 
    // ===============================
    // DEPENDENCIES
    // ===============================
 
    CommChannelsManager* comm_manager_ = nullptr;
 
    GpioManager* gpio_manager_ = nullptr;
 
    // PCAL95555 pins from GpioManager (non-owning; valid while GpioManager is initialised).
    BaseGpio* onboard_bno_reset_gpio_{nullptr};
    BaseGpio* onboard_bno_boot_gpio_{nullptr};
    BaseGpio* onboard_bno_int_gpio_{nullptr};
 
    // ===============================
    // INTERRUPT SUPPORT
    // ===============================
 
    BaseGpio* interrupt_gpio_ = nullptr;                    
    ImuInterruptCallback interrupt_callback_{nullptr};      
    void* interrupt_callback_user_{nullptr};
    bool interrupt_configured_ = false;
    bool interrupt_enabled_ = false;
    std::atomic<uint32_t> interrupt_count_{0};
    SignalSemaphore interrupt_semaphore_{"VxImuInt"};  
    bool interrupt_semaphore_ready_ = false;
};
 
//==============================================================================
// CONVENIENCE
//==============================================================================
 
[[nodiscard]] inline ImuManager& GetImuManager() noexcept {
    return ImuManager::GetInstance();
}