Tmc9660DriveObservability.h

Go to the documentation of this file.

 
#ifndef VORTEX_TMC9660_DRIVE_OBSERVABILITY_H_
#define VORTEX_TMC9660_DRIVE_OBSERVABILITY_H_
 
#include <algorithm>
#include <array>
#include <cstddef>
#include <cstdint>
#include <span>
 
#include "bsp_bind/VortexBoardBind.h"  // MonotonicMicros()
#include "core/hf-core-drivers/external/hf-tmc9660-driver/inc/tmc9660.hpp"
 
namespace vortex::motor {
 
using Param = tmc9660::tmcl::Parameters;
using ReplyCode = tmc9660::tmcl::ReplyCode;
 
//==============================================================================
// Batch access
//==============================================================================
 
struct ParamAccess {
    Param id{};                                   
    uint32_t value{0};                            
    uint8_t motor{0};                             
    ReplyCode status{ReplyCode::REPLY_CHKERR};    
    [[nodiscard]] bool ok() const noexcept { return status == ReplyCode::REPLY_OK; }
};
 
inline constexpr size_t kBatchChunk = 16;
 
namespace detail {
 
template <typename Driver, bool kWrite>
size_t RunBatch(Driver& driver, std::span<ParamAccess> items) noexcept {
    using DriverAccess = typename Driver::ParameterAccess;
    std::array<DriverAccess, kBatchChunk> buf{};
    size_t ok = 0;
    for (size_t base = 0; base < items.size(); base += kBatchChunk) {
        const size_t n = std::min(kBatchChunk, items.size() - base);
        for (size_t i = 0; i < n; ++i) {
            buf[i] = DriverAccess{};
            buf[i].id = items[base + i].id;
            buf[i].value = items[base + i].value;
            buf[i].motor = items[base + i].motor;
        }
        const std::span<DriverAccess> chunk(buf.data(), n);
        if constexpr (kWrite) {
            (void)driver.writeParameters(chunk);
        } else {
            (void)driver.readParameters(chunk);
        }
        for (size_t i = 0; i < n; ++i) {
            ParamAccess& out = items[base + i];
            out.status = buf[i].status;
            if constexpr (!kWrite) {
                if (buf[i].ok()) {
                    out.value = buf[i].value;
                }
            }
            ok += out.ok() ? 1 : 0;
        }
    }
    return ok;
}
 
}  // namespace detail
 
template <typename Driver>
size_t BatchRead(Driver& driver, std::span<ParamAccess> items) noexcept {
    return detail::RunBatch<Driver, false>(driver, items);
}
 
template <typename Driver>
size_t BatchWrite(Driver& driver, std::span<ParamAccess> items) noexcept {
    return detail::RunBatch<Driver, true>(driver, items);
}
 
//==============================================================================
// Telemetry snapshot
//==============================================================================
 
enum class TelemetryField : uint8_t {
    ActualPosition = 0,     
    ActualVelocity,         
    TorqueFluxActual,       
    GeneralStatus,          
    GeneralErrors,          
    GateDriverErrors,       
    AdcStatus,              
    SupplyVoltage,          
    ChipTemperature,        
    ExternalTemperature,    
    TotalMotorCurrent,      
    RampVelocity,           
    RampPosition,           
    VoltageDqActual,        
    Iit1Sum,                
    Iit2Sum,                
    Count
};
 
inline constexpr size_t kTelemetryFieldCount = static_cast<size_t>(TelemetryField::Count);
 
inline constexpr std::array<Param, kTelemetryFieldCount> kTelemetryParam{
    Param::ACTUAL_POSITION,     Param::ACTUAL_VELOCITY,
    Param::TORQUE_FLUX_COMBINED_ACTUAL_VALUES,
    Param::GENERAL_STATUS_FLAGS, Param::GENERAL_ERROR_FLAGS, Param::GDRV_ERROR_FLAGS, Param::ADC_STATUS_FLAGS,
    Param::SUPPLY_VOLTAGE,      Param::CHIP_TEMPERATURE,    Param::EXTERNAL_TEMPERATURE,
    Param::ACTUAL_TOTAL_MOTOR_CURRENT,
    Param::RAMP_VELOCITY,       Param::RAMP_POSITION,
    Param::VOLTAGE_D_Q_COMBINED_ACTUAL_VALUES,
    Param::IIT_SUM_1,           Param::IIT_SUM_2,
};
 
enum class TelemetrySet : uint8_t {
    Cyclic = 0,
    Status,
    Full,
};
 
struct MotorTelemetrySnapshot {
    std::array<uint32_t, kTelemetryFieldCount> raw{};        
    std::array<uint32_t, kTelemetryFieldCount> field_seq{};  
    uint32_t sequence{0};            
    uint32_t valid_mask{0};          
    uint32_t requested_mask{0};      
    uint64_t timestamp_us{0};        
    uint32_t batch_duration_us{0};   
    uint8_t accesses{0};             
    uint8_t failed{0};               
 
    [[nodiscard]] uint32_t Raw(TelemetryField f) const noexcept { return raw[static_cast<size_t>(f)]; }
    [[nodiscard]] bool Valid(TelemetryField f) const noexcept {
        return (valid_mask >> static_cast<unsigned>(f)) & 1U;
    }
    [[nodiscard]] uint32_t Age(TelemetryField f) const noexcept {
        const uint32_t s = field_seq[static_cast<size_t>(f)];
        return s == 0 ? UINT32_MAX : sequence - s;
    }
 
    [[nodiscard]] int32_t ActualPosition() const noexcept {
        return static_cast<int32_t>(Raw(TelemetryField::ActualPosition));
    }
    [[nodiscard]] int32_t ActualVelocity() const noexcept {
        return static_cast<int32_t>(Raw(TelemetryField::ActualVelocity));
    }
    [[nodiscard]] int16_t TorqueCurrentRaw() const noexcept {
        return static_cast<int16_t>(Raw(TelemetryField::TorqueFluxActual) >> 16);
    }
    [[nodiscard]] int16_t FluxCurrentRaw() const noexcept {
        return static_cast<int16_t>(Raw(TelemetryField::TorqueFluxActual) & 0xFFFFU);
    }
    [[nodiscard]] float SupplyVoltageV() const noexcept {
        return static_cast<float>(Raw(TelemetryField::SupplyVoltage)) * 0.1f;
    }
    [[nodiscard]] float ChipTemperatureC() const noexcept {
        return static_cast<float>(Raw(TelemetryField::ChipTemperature)) * 0.01615f - 268.15f;
    }
    [[nodiscard]] int32_t TotalMotorCurrentMa() const noexcept {
        return static_cast<int32_t>(Raw(TelemetryField::TotalMotorCurrent));
    }
};
 
[[nodiscard]] constexpr uint32_t TelemetryRequestMask(TelemetrySet set, uint32_t next_sequence) noexcept {
    auto bit = [](TelemetryField f) constexpr { return 1U << static_cast<unsigned>(f); };
    switch (set) {
        case TelemetrySet::Cyclic:
            return bit(TelemetryField::ActualPosition) | bit(TelemetryField::ActualVelocity) |
                   bit(TelemetryField::TorqueFluxActual) | bit(TelemetryField::GeneralStatus) |
                   ((next_sequence & 1U) ? bit(TelemetryField::GeneralErrors)
                                         : bit(TelemetryField::GateDriverErrors));
        case TelemetrySet::Status:
            return bit(TelemetryField::GeneralStatus) | bit(TelemetryField::GeneralErrors) |
                   bit(TelemetryField::GateDriverErrors) | bit(TelemetryField::AdcStatus) |
                   bit(TelemetryField::SupplyVoltage) | bit(TelemetryField::ChipTemperature) |
                   bit(TelemetryField::ExternalTemperature) | bit(TelemetryField::TotalMotorCurrent) |
                   bit(TelemetryField::Iit1Sum) | bit(TelemetryField::Iit2Sum);
        default:
            return (1U << kTelemetryFieldCount) - 1U;
    }
}
 
template <typename Driver>
size_t ReadTelemetry(Driver& driver, TelemetrySet set, MotorTelemetrySnapshot& snap) noexcept {
    const uint32_t seq = snap.sequence + 1;
    const uint32_t mask = TelemetryRequestMask(set, seq);
    std::array<ParamAccess, kTelemetryFieldCount> items{};
    std::array<uint8_t, kTelemetryFieldCount> field_of{};
    size_t n = 0;
    for (size_t f = 0; f < kTelemetryFieldCount; ++f) {
        if ((mask >> f) & 1U) {
            items[n].id = kTelemetryParam[f];
            field_of[n] = static_cast<uint8_t>(f);
            ++n;
        }
    }
    const uint64_t t0 = vortex::bsp::MonotonicMicros();
    const size_t ok = BatchRead(driver, std::span<ParamAccess>(items.data(), n));
    const uint64_t t1 = vortex::bsp::MonotonicMicros();
 
    snap.sequence = seq;
    snap.requested_mask = mask;
    snap.valid_mask = 0;
    for (size_t i = 0; i < n; ++i) {
        if (items[i].ok()) {
            const size_t f = field_of[i];
            snap.raw[f] = items[i].value;
            snap.field_seq[f] = seq;
            snap.valid_mask |= 1U << f;
        }
    }
    snap.timestamp_us = t1;
    snap.batch_duration_us = static_cast<uint32_t>(t1 - t0);
    snap.accesses = static_cast<uint8_t>(n);
    snap.failed = static_cast<uint8_t>(n - ok);
    return ok;
}
 
//==============================================================================
// Fault decoding
//==============================================================================
 
enum class DriveSeverity : uint8_t {
    Ok = 0,      
    Warning,     
    Fault,       
};
 
struct MotorDriveDiagnostics {
    uint32_t general_status{0};       
    uint32_t general_errors{0};       
    uint32_t gate_driver_errors{0};   
    uint32_t adc_status{0};           
    uint8_t words_valid{0};           
 
    // General status (289)
    bool regulation_stopped{false};   
    bool adc_offset_calibrated{false};
    bool iit1_active{false};          
    bool iit2_active{false};          
    bool config_loaded{false};        
 
    // General errors (299)
    bool chip_reset_detected{false};  
    bool hall_invalid{false};         
    bool ext_overtemperature{false};  
    bool chip_overtemperature{false}; 
    bool iit1_exceeded{false};        
    bool iit2_exceeded{false};        
    bool ext_temperature_warning{false};   
    bool supply_overvoltage_warning{false};
    bool supply_undervoltage_warning{false};
    bool adc_input_overvoltage{false};     
    bool fault_retry_happened{false};      
    bool fault_retries_failed{false};      
    bool chip_temperature_warning{false};  
    bool heartbeat_stopped{false};         
 
    // Gate driver errors (300), grouped by kind; bit order per kind is U, V, W, Y2
    uint8_t overcurrent_low_side{0};   
    uint8_t overcurrent_high_side{0};  
    uint8_t vgs_short_low_side{0};     
    uint8_t vgs_short_high_side{0};    
    uint8_t bootstrap_undervoltage{0}; 
    bool gate_driver_undervoltage{false}; 
 
    // ADC (301)
    uint16_t adc_clipped{0};          
 
    DriveSeverity severity{DriveSeverity::Ok};
 
    // Link
    uint32_t batch_duration_us{0};    
    bool spi_stats_valid{false};      
    tmc9660::TmclSpiStats spi_stats{};
};
 
[[nodiscard]] inline MotorDriveDiagnostics DecodeDriveStatus(uint32_t status, uint32_t gen_err, uint32_t gdrv,
                                                             uint32_t adc) noexcept {
    using tmc9660::tmcl::GeneralErrorFlags;
    using tmc9660::tmcl::GeneralStatusFlags;
    auto s = [status](GeneralStatusFlags f) { return (status & static_cast<uint32_t>(f)) != 0; };
    auto e = [gen_err](GeneralErrorFlags f) { return (gen_err & static_cast<uint32_t>(f)) != 0; };
    auto nib = [](uint32_t w, unsigned shift) { return static_cast<uint8_t>((w >> shift) & 0xFU); };
 
    MotorDriveDiagnostics d{};
    d.general_status = status;
    d.general_errors = gen_err;
    d.gate_driver_errors = gdrv;
    d.adc_status = adc;
 
    d.regulation_stopped = s(GeneralStatusFlags::REGULATION_STOPPED);
    d.adc_offset_calibrated = s(GeneralStatusFlags::ADC_OFFSET_CALIBRATED);
    d.iit1_active = s(GeneralStatusFlags::IIT_1_ACTIVE);
    d.iit2_active = s(GeneralStatusFlags::IIT_2_ACTIVE);
    d.config_loaded = s(GeneralStatusFlags::CONFIG_LOADED);
 
    d.chip_reset_detected = e(GeneralErrorFlags::WATCHDOG_EVENT);
    d.hall_invalid = e(GeneralErrorFlags::HALL_ERROR);
    d.ext_overtemperature = e(GeneralErrorFlags::EXT_TEMP_EXCEEDED);
    d.chip_overtemperature = e(GeneralErrorFlags::CHIP_TEMP_EXCEEDED);
    d.iit1_exceeded = e(GeneralErrorFlags::I2T_1_EXCEEDED);
    d.iit2_exceeded = e(GeneralErrorFlags::I2T_2_EXCEEDED);
    d.ext_temperature_warning = e(GeneralErrorFlags::EXT_TEMP_WARNING);
    d.supply_overvoltage_warning = e(GeneralErrorFlags::SUPPLY_OVERVOLTAGE_WARNING);
    d.supply_undervoltage_warning = e(GeneralErrorFlags::SUPPLY_UNDERVOLTAGE_WARNING);
    d.adc_input_overvoltage = e(GeneralErrorFlags::ADC_IN_OVERVOLTAGE);
    d.fault_retry_happened = e(GeneralErrorFlags::FAULT_RETRY_HAPPENED);
    d.fault_retries_failed = e(GeneralErrorFlags::FAULT_RETRIES_FAILED);
    d.chip_temperature_warning = e(GeneralErrorFlags::CHIP_TEMP_WARNING);
    d.heartbeat_stopped = e(GeneralErrorFlags::HEARTBEAT_STOPPED);
 
    d.overcurrent_low_side = nib(gdrv, 0);
    d.vgs_short_low_side = static_cast<uint8_t>(nib(gdrv, 4) | nib(gdrv, 8));
    d.bootstrap_undervoltage = nib(gdrv, 12);
    d.overcurrent_high_side = nib(gdrv, 16);
    d.vgs_short_high_side = static_cast<uint8_t>(nib(gdrv, 20) | nib(gdrv, 24));
    d.gate_driver_undervoltage = (gdrv & 0xE0000000U) != 0;
 
    d.adc_clipped = static_cast<uint16_t>(adc & 0x3FFFU);
 
    const bool fault = gdrv != 0 || d.fault_retries_failed || d.ext_overtemperature || d.chip_overtemperature ||
                       d.heartbeat_stopped || d.iit1_exceeded || d.iit2_exceeded;
    const bool warning = d.chip_reset_detected || d.hall_invalid || d.ext_temperature_warning ||
                         d.supply_overvoltage_warning || d.supply_undervoltage_warning || d.adc_input_overvoltage ||
                         d.fault_retry_happened || d.chip_temperature_warning || d.config_loaded ||
                         d.adc_clipped != 0;
    d.severity = fault ? DriveSeverity::Fault : (warning ? DriveSeverity::Warning : DriveSeverity::Ok);
    return d;
}
 
template <typename Driver>
bool ReadDriveDiagnostics(Driver& driver, MotorDriveDiagnostics& out) noexcept {
    std::array<ParamAccess, 4> items{};
    items[0].id = Param::GENERAL_STATUS_FLAGS;
    items[1].id = Param::GENERAL_ERROR_FLAGS;
    items[2].id = Param::GDRV_ERROR_FLAGS;
    items[3].id = Param::ADC_STATUS_FLAGS;
    const uint64_t t0 = vortex::bsp::MonotonicMicros();
    (void)BatchRead(driver, std::span<ParamAccess>(items));
    const uint64_t t1 = vortex::bsp::MonotonicMicros();
    out = DecodeDriveStatus(items[0].value, items[1].value, items[2].value, items[3].value);
    out.words_valid = 0;
    for (size_t i = 0; i < items.size(); ++i) {
        out.words_valid = static_cast<uint8_t>(out.words_valid | (items[i].ok() ? (1U << i) : 0U));
    }
    out.batch_duration_us = static_cast<uint32_t>(t1 - t0);
    return out.words_valid == 0xFU;
}
 
template <typename Driver>
bool ClearLatchedFlags(Driver& driver, uint32_t general_errors, uint32_t gate_driver_errors,
                       uint32_t adc_status) noexcept {
    std::array<ParamAccess, 3> items{};
    size_t n = 0;
    if (general_errors != 0) items[n++] = ParamAccess{Param::GENERAL_ERROR_FLAGS, general_errors, 0, {}};
    if (gate_driver_errors != 0) items[n++] = ParamAccess{Param::GDRV_ERROR_FLAGS, gate_driver_errors, 0, {}};
    if (adc_status != 0) items[n++] = ParamAccess{Param::ADC_STATUS_FLAGS, adc_status, 0, {}};
    return BatchWrite(driver, std::span<ParamAccess>(items.data(), n)) == n;
}
 
//==============================================================================
// Configuration: parameter image apply
//==============================================================================
 
struct ParamWrite {
    Param id{};
    uint32_t value{0};
};
 
struct ApplyOptions {
    bool differential{true};  
    bool verify{true};        
};
 
struct ApplyResult {
    uint16_t entries{0};          
    uint16_t written{0};          
    uint16_t skipped{0};          
    uint16_t rejected{0};         
    uint16_t mismatches{0};       
    Param first_bad{};            
    ReplyCode first_bad_status{ReplyCode::REPLY_OK};
    uint32_t first_bad_readback{0};
    [[nodiscard]] bool ok() const noexcept { return rejected == 0 && mismatches == 0; }
};
 
namespace detail {
 
template <typename Driver, typename Pred>
void ApplyPhase(Driver& driver, std::span<const ParamWrite> image, ApplyOptions opt, Pred take,
                ApplyResult& r) noexcept {
    auto note_bad = [&r](Param id, ReplyCode status, uint32_t readback) {
        if (r.rejected + r.mismatches == 0) {
            r.first_bad = id;
            r.first_bad_status = status;
            r.first_bad_readback = readback;
        }
    };
    std::array<ParamAccess, kBatchChunk> chunk{};
    size_t i = 0;
    while (i < image.size()) {
        // Gather the next chunk of selected entries.
        size_t n = 0;
        while (i < image.size() && n < kBatchChunk) {
            if (take(image[i])) {
                chunk[n++] = ParamAccess{image[i].id, image[i].value, 0, ReplyCode::REPLY_CHKERR};
            }
            ++i;
        }
        if (n == 0) {
            continue;
        }
        // Differential: drop entries the chip already holds.
        std::array<ParamAccess, kBatchChunk> to_write{};
        size_t w = 0;
        if (opt.differential) {
            std::array<ParamAccess, kBatchChunk> current{};
            for (size_t j = 0; j < n; ++j) current[j].id = chunk[j].id;
            (void)BatchRead(driver, std::span<ParamAccess>(current.data(), n));
            for (size_t j = 0; j < n; ++j) {
                if (current[j].ok() && current[j].value == chunk[j].value) {
                    ++r.skipped;
                } else {
                    to_write[w++] = chunk[j];
                }
            }
        } else {
            for (size_t j = 0; j < n; ++j) to_write[w++] = chunk[j];
        }
        (void)BatchWrite(driver, std::span<ParamAccess>(to_write.data(), w));
        r.written = static_cast<uint16_t>(r.written + w);
        for (size_t j = 0; j < w; ++j) {
            if (!to_write[j].ok()) {
                note_bad(to_write[j].id, to_write[j].status, 0);
                ++r.rejected;
            }
        }
        // Verify: read the whole chunk back.
        if (opt.verify) {
            for (size_t j = 0; j < n; ++j) {
                to_write[j] = ParamAccess{chunk[j].id, 0, 0, ReplyCode::REPLY_CHKERR};
            }
            (void)BatchRead(driver, std::span<ParamAccess>(to_write.data(), n));
            for (size_t j = 0; j < n; ++j) {
                if (!to_write[j].ok() || to_write[j].value != chunk[j].value) {
                    note_bad(chunk[j].id, to_write[j].status, to_write[j].value);
                    ++r.mismatches;
                }
            }
        }
    }
}
 
}  // namespace detail
 
template <typename Driver>
ApplyResult ApplyParameterImage(Driver& driver, std::span<const ParamWrite> image, ApplyOptions opt) noexcept {
    ApplyResult r{};
    r.entries = static_cast<uint16_t>(std::min<size_t>(image.size(), UINT16_MAX));
    detail::ApplyPhase(driver, image, opt, [](const ParamWrite& e) { return e.id == Param::MOTOR_TYPE; }, r);
    detail::ApplyPhase(driver, image, opt, [](const ParamWrite& e) { return e.id != Param::MOTOR_TYPE; }, r);
    return r;
}
 
}  // namespace vortex::motor
 
#endif  // VORTEX_TMC9660_DRIVE_OBSERVABILITY_H_