MotorController.h

Go to the documentation of this file.

#ifndef COMPONENT_HANDLER_MOTOR_CONTROLLER_H_
#define COMPONENT_HANDLER_MOTOR_CONTROLLER_H_
 
#include "handlers/tmc9660/Tmc9660Handler.h"
#include "CommChannelsManager.h"
#include "RtosMutex.h"
#include "api_helpers/Tmc9660DriveObservability.h"
#include <span>
#include <array>
#include <atomic>
#include <optional>
 
struct Tmc9660ControlPins {
    BaseGpio& rst;      
    BaseGpio& drv_en;   
    BaseGpio& faultn;   
    BaseGpio& wake;     
};
 
//==============================================================================
// MOTOR MECHANICAL FRAME (gearbox / output shaft mapping)
//==============================================================================
 
struct MotorMechanics {
    double gear_ratio_motor_to_output{1.0};
 
    bool invert_output{false};
};
 
//==============================================================================
// DRIVE OBSERVABILITY TYPES (see api_helpers/Tmc9660DriveObservability.h)
//==============================================================================
 
using MotorParameterAccess = vortex::motor::ParamAccess;          
using MotorParameterWrite = vortex::motor::ParamWrite;            
using MotorTelemetrySet = vortex::motor::TelemetrySet;            
using MotorTelemetryField = vortex::motor::TelemetryField;
using MotorTelemetrySnapshot = vortex::motor::MotorTelemetrySnapshot;
using MotorDriveDiagnostics = vortex::motor::MotorDriveDiagnostics;
using MotorApplyOptions = vortex::motor::ApplyOptions;
using MotorApplyResult = vortex::motor::ApplyResult;
 
struct MotorAccessStats {
    uint32_t batches{0};            
    uint32_t accesses{0};           
    uint32_t failed_accesses{0};    
    uint32_t last_batch_us{0};      
    uint32_t max_batch_us{0};       
};
 
//==============================================================================
// MOTOR CONTROLLER ERROR CODES
//==============================================================================
 
enum class MotorError : uint8_t {
    SUCCESS = 0,              
    NOT_INITIALIZED,          
    INITIALIZATION_FAILED,    
    DEVICE_ALREADY_EXISTS,    
    DEVICE_NOT_FOUND,         
    INVALID_DEVICE_INDEX,     
    CANNOT_DELETE_ONBOARD,    
    DEPENDENCY_NOT_READY,     
    COMMUNICATION_FAILED,     
    INVALID_PARAMETER,        
    HANDLER_CREATION_FAILED,  
    MUTEX_LOCK_FAILED         
};
 
enum class Tmc9660HostSpiGateMode : uint8_t {
    Disabled = 0,                 
    EnabledForHostSpiMotor = 1    
};
 
constexpr const char* MotorErrorToString(MotorError error) noexcept {
    switch (error) {
        case MotorError::SUCCESS:                 return "Success";
        case MotorError::NOT_INITIALIZED:         return "Not initialized";
        case MotorError::INITIALIZATION_FAILED:   return "Initialization failed";
        case MotorError::DEVICE_ALREADY_EXISTS:   return "Device already exists";
        case MotorError::DEVICE_NOT_FOUND:        return "Device not found";
        case MotorError::INVALID_DEVICE_INDEX:    return "Invalid device index";
        case MotorError::CANNOT_DELETE_ONBOARD:   return "Cannot delete onboard device";
        case MotorError::DEPENDENCY_NOT_READY:    return "Dependency not ready";
        case MotorError::COMMUNICATION_FAILED:    return "Communication failed";
        case MotorError::INVALID_PARAMETER:       return "Invalid parameter";
        case MotorError::HANDLER_CREATION_FAILED: return "Handler creation failed";
        case MotorError::MUTEX_LOCK_FAILED:       return "Mutex lock failed";
        default:                                  return "Unknown error";
    }
}
 
//==============================================================================
// MOTOR CONTROLLER DIAGNOSTICS
//==============================================================================
 
struct MotorSystemDiagnostics {
    bool system_healthy;               
    bool system_initialized;           
    uint8_t active_device_count;       
    uint8_t initialized_device_count;  
    MotorError last_error;             
 
    struct DeviceSnapshot {
        bool active;
        bool initialized;
        uint32_t error_count;          
    };
    static constexpr uint8_t kMaxDevices = 4;
    DeviceSnapshot devices[kMaxDevices]{};
};
 
class MotorController {
public:
    static constexpr uint8_t MAX_TMC9660_DEVICES = 4;   
    static constexpr uint8_t ONBOARD_TMC9660_INDEX = 0; 
    static constexpr uint8_t EXTERNAL_DEVICE_1_INDEX = 2; 
    static constexpr uint8_t EXTERNAL_DEVICE_2_INDEX = 3; 
public:
    static MotorController& GetInstance() noexcept;
 
    //**************************************************************************//
    //**                  DEVICE MANAGEMENT METHODS                           **//
    //**************************************************************************//
 
    bool EnsureInitialized() noexcept {
        if (!initialized_.load(std::memory_order_acquire)) {
            RtosUniqueLock<RtosMutex> lock(deviceMutex_);
            if (!initialized_.load(std::memory_order_relaxed)) {
                initialized_.store(Initialize(), std::memory_order_release);
            }
        }
        return initialized_.load(std::memory_order_acquire);
    }
 
    inline bool IsInitialized() const noexcept { return initialized_.load(std::memory_order_acquire); }
 
    bool Deinitialize() noexcept;
 
    [[nodiscard]] MotorError GetLastError() const noexcept { return last_error_.load(std::memory_order_acquire); }
 
    [[nodiscard]] MotorError GetSystemDiagnostics(MotorSystemDiagnostics& diagnostics) const noexcept;
 
    [[nodiscard]] MotorError ApplyOnboardTmc9660HostSpiGateMode(Tmc9660HostSpiGateMode mode) noexcept;
 
    [[nodiscard]] std::optional<tmc9660::CommMode> TryGetOnboardCommMode() const noexcept;
 
    //**************************************************************************//
    //**                  TMCL SPI TIMING (BENCH TUNING)                      **//
    //**************************************************************************//
 
    [[nodiscard]] MotorError SetTmclSpiTiming(const tmc9660::TmclSpiTiming& timing,
                                              uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    [[nodiscard]] MotorError GetTmclSpiTiming(tmc9660::TmclSpiTiming& timing,
                                              uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) const noexcept;
 
    [[nodiscard]] MotorError GetTmclSpiStats(tmc9660::TmclSpiStats& stats,
                                             uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    [[nodiscard]] MotorError ResetTmclSpiStats(uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    //**************************************************************************//
    //**        DRIVE OBSERVABILITY: BATCH ACCESS, TELEMETRY, DIAGNOSTICS    **//
    //**************************************************************************//
    // Every call below is one (or, for an image, a few) pipelined TMCL batch, sent through the
    // handler's visitDriver() while this manager's device lock is held. A device that is not
    // active returns DEVICE_NOT_FOUND; one whose bring-up did not succeed returns NOT_INITIALIZED
    // (no call here starts a bootloader bring-up). COMMUNICATION_FAILED means at least one access
    // was not answered REPLY_OK; per-element results are in the items / snapshot / result.
 
    [[nodiscard]] MotorError ReadParameters(std::span<MotorParameterAccess> items,
                                            uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    [[nodiscard]] MotorError WriteParameters(std::span<MotorParameterAccess> items,
                                             uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    [[nodiscard]] MotorError ReadTelemetry(MotorTelemetrySet set, MotorTelemetrySnapshot& snapshot,
                                           uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    [[nodiscard]] MotorError ReadDriveDiagnostics(MotorDriveDiagnostics& out,
                                                  uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    [[nodiscard]] MotorError ClearLatchedFaults(uint32_t general_errors, uint32_t gate_driver_errors,
                                                uint32_t adc_status,
                                                uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    [[nodiscard]] MotorError ApplyParameterImage(std::span<const MotorParameterWrite> image,
                                                 const MotorApplyOptions& options, MotorApplyResult& result,
                                                 uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    [[nodiscard]] MotorError GetAccessStats(MotorAccessStats& stats,
                                            uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) const noexcept;
 
    void ResetAccessStats(uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    //**************************************************************************//
    //**                  HANDLER AND DRIVER MANAGEMENT                       **//
    //**************************************************************************//
 
    Tmc9660Handler* handler(uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    template <typename Func>
    auto visitDriver(Func&& func, uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept {
        // Hold the device lock for the whole visit, so the slot cannot be deleted underneath it;
        // the handler takes its own lock inside (order: deviceMutex_ -> handler mutex).
        MutexLockGuard lock(deviceMutex_);
        Tmc9660Handler* h = (deviceIndex < MAX_TMC9660_DEVICES && deviceActive_[deviceIndex] &&
                             tmcHandlers_[deviceIndex])
                                ? &*tmcHandlers_[deviceIndex]
                                : nullptr;
        if (h) {
            return h->visitDriver(std::forward<Func>(func));
        }
        // Driver not available - return default
        using ReturnType = decltype(h->visitDriver(std::forward<Func>(func)));
        if constexpr (std::is_void_v<ReturnType>) {
            return;
        } else {
            return ReturnType{};
        }
    }
 
    //**************************************************************************//
    //**            MECHANICAL FRAME / GEARBOX (Motor <-> Load)               **//
    //**************************************************************************//
 
    bool setMotorMechanics(const MotorMechanics& mech,
                           uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    [[nodiscard]] MotorMechanics getMotorMechanics(
        uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) const noexcept;
 
    //**************************************************************************//
    //**            MOTOR-FRAME COMMANDS (rotor-side, raw chip frame)         **//
    //**************************************************************************//
 
    bool setMotorVelocity(double value, ::tmc9660::units::VelocityUnit unit,
                          ::tmc9660::units::MotorContext const& ctx,
                          uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    bool getMotorVelocity(double& out_value, ::tmc9660::units::VelocityUnit unit,
                          ::tmc9660::units::MotorContext const& ctx,
                          uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    bool setMotorPosition(double value, ::tmc9660::units::PositionUnit unit,
                          ::tmc9660::units::MotorContext const& ctx,
                          uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    bool getMotorPosition(double& out_value, ::tmc9660::units::PositionUnit unit,
                          ::tmc9660::units::MotorContext const& ctx,
                          uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    //**************************************************************************//
    //**            LOAD-FRAME COMMANDS (output-shaft frame, gearbox-aware)   **//
    //**************************************************************************//
 
    bool setLoadVelocity(double value, ::tmc9660::units::VelocityUnit unit,
                         ::tmc9660::units::MotorContext const& ctx,
                         uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    bool getLoadVelocity(double& out_value, ::tmc9660::units::VelocityUnit unit,
                         ::tmc9660::units::MotorContext const& ctx,
                         uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    bool setLoadPosition(double value, ::tmc9660::units::PositionUnit unit,
                         ::tmc9660::units::MotorContext const& ctx,
                         uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    bool getLoadPosition(double& out_value, ::tmc9660::units::PositionUnit unit,
                         ::tmc9660::units::MotorContext const& ctx,
                         uint8_t deviceIndex = ONBOARD_TMC9660_INDEX) noexcept;
 
    //**************************************************************************//
    //**                  DEVICES MANAGEMENT METHODS                           **//
    //**************************************************************************//
 
    [[nodiscard]] MotorError CreateOnboardDevice(BaseSpi& spiInterface, 
                            uint8_t address,
                            const Tmc9660ControlPins& pins,
                            const tmc9660::BootloaderConfig* bootCfg = nullptr) noexcept;
 
    [[nodiscard]] MotorError CreateOnboardDevice(BaseUart& uartInterface,
                            uint8_t address,
                            const Tmc9660ControlPins& pins,
                            const tmc9660::BootloaderConfig* bootCfg = nullptr) noexcept;
 
    [[nodiscard]] MotorError CreateExternalDevice(uint8_t csDeviceIndex, 
                            SpiDeviceId spiDeviceId, 
                            uint8_t address,
                            const Tmc9660ControlPins& pins,
                            const tmc9660::BootloaderConfig* bootCfg = nullptr) noexcept;
 
    [[nodiscard]] MotorError CreateExternalDevice(uint8_t csDeviceIndex,
                            BaseUart& uartInterface,
                            uint8_t address,
                            const Tmc9660ControlPins& pins,
                            const tmc9660::BootloaderConfig* bootCfg = nullptr) noexcept;
 
    [[nodiscard]] MotorError DeleteExternalDevice(uint8_t csDeviceIndex) noexcept;
 
    uint8_t GetDeviceCount() const noexcept;
 
    bool IsDeviceValid(uint8_t deviceIndex) const noexcept;
 
    bool IsExternalSlotAvailable(uint8_t csDeviceIndex) const noexcept;
 
    void GetActiveDeviceIndices(std::array<uint8_t, MAX_TMC9660_DEVICES>& out, size_t& count) const noexcept;
 
    size_t InitializeAllDevices(std::array<bool, MAX_TMC9660_DEVICES>& results) noexcept;
 
    void GetInitializationStatus(std::array<bool, MAX_TMC9660_DEVICES>& status) const noexcept;
    
    void DumpStatistics() const noexcept;
 
private:
    MotorController();
    ~MotorController() = default;
    MotorController(const MotorController&) = delete;
    MotorController& operator=(const MotorController&) = delete;
 
    bool Initialize();
 
    bool IsExternalDeviceIndex(uint8_t csDeviceIndex) const noexcept;
 
    void UpdateLastError(MotorError error, int deviceIndex = -1) noexcept;
 
    void configureOnboardHostBusBeforeHandlerInitLocked(uint8_t device_index) noexcept;
 
    [[nodiscard]] MotorError applyOnboardTmc9660HostSpiGateModeLocked(Tmc9660HostSpiGateMode mode) noexcept;
 
    void applyTmclSpiTimingLocked(uint8_t device_index) noexcept;
 
    template <typename Fn>
    MotorError withReadyDriverLocked(uint8_t device_index, Fn&& fn) noexcept {
        if (device_index >= MAX_TMC9660_DEVICES || !deviceActive_[device_index] || !tmcHandlers_[device_index]) {
            return MotorError::DEVICE_NOT_FOUND;
        }
        // Never start a bootloader bring-up from an observability call: only drivers that are up.
        if (!tmcHandlers_[device_index]->IsDriverReady()) {
            return MotorError::NOT_INITIALIZED;
        }
        bool ran = false;
        tmcHandlers_[device_index]->visitDriver([&](auto& driver) {
            ran = true;
            fn(driver);
        });
        return ran ? MotorError::SUCCESS : MotorError::NOT_INITIALIZED;
    }
 
    void recordBatchLocked(uint8_t device_index, size_t accesses, size_t failed, uint32_t duration_us) noexcept;
 
    template <typename Fn>
    bool withSpiDriverLocked(uint8_t device_index, Fn&& fn) noexcept {
        // A handler that is not up would run its full bootloader init inside visitDriver(); that
        // belongs to the init paths (PCAL SPI mux first), so only ready drivers are touched here.
        if (device_index >= MAX_TMC9660_DEVICES || !deviceActive_[device_index] || !tmcHandlers_[device_index] ||
            !tmcHandlers_[device_index]->IsDriverReady()) {
            return false;
        }
        bool ran = false;
        tmcHandlers_[device_index]->visitDriver([&](auto& driver) {
            if constexpr (std::is_same_v<std::decay_t<decltype(driver)>, Tmc9660Handler::SpiDriver>) {
                ran = true;
                fn(driver);
            }
        });
        return ran;
    }
 
    std::array<std::optional<Tmc9660Handler>, MAX_TMC9660_DEVICES> tmcHandlers_;
    std::array<bool, MAX_TMC9660_DEVICES> deviceInitialized_;
    std::array<bool, MAX_TMC9660_DEVICES> deviceActive_;    
    std::array<std::atomic<uint32_t>, MAX_TMC9660_DEVICES> deviceErrorCounts_{}; 
    std::array<MotorMechanics, MAX_TMC9660_DEVICES> mechanics_{}; 
    std::array<tmc9660::TmclSpiTiming, MAX_TMC9660_DEVICES> tmclSpiTiming_{}; 
    std::array<MotorAccessStats, MAX_TMC9660_DEVICES> accessStats_{};          
    
    bool onboardDeviceCreated_;         
    std::atomic<bool> initialized_;      
    std::atomic<MotorError> last_error_{MotorError::SUCCESS}; 
    mutable RtosMutex deviceMutex_;     
};
 
//==============================================================================
// CONVENIENCE
//==============================================================================
 
[[nodiscard]] inline MotorController& GetMotorController() noexcept {
    return MotorController::GetInstance();
}
 
#endif // COMPONENT_HANDLER_MOTOR_CONTROLLER_H_