MFEAxisPositionMonitor.cpp

/*
** Copyright (C) 2025 CERN
**
** This software is provided 'as-is', without any express or implied
** warranty.  In no event will the authors be held liable for any damages
** arising from the use of this software.
**
** Permission is granted to anyone to use this software for any purpose,
** including commercial applications, and to alter it and redistribute it
** freely, subject to the following restrictions:
**
** 1. The origin of this software must not be misrepresented; you must not
**    claim that you wrote the original software. If you use this software
**    in a product, an acknowledgment in the product documentation would be
**    appreciated but is not required.
** 2. Altered source versions must be plainly marked as such, and must not be
**    misrepresented as being the original software.
** 3. This notice may not be removed or altered from any source distribution.
**
** Created on: 2025-04-17T18:13:59
** Author: Sylvain Fargier <sylvain.fargier@cern.ch>
*/
 
#include "MFEAxisPositionMonitor.hpp"
 
#include <ccut/async.hpp>
#include <ccut/utils.hpp>
 
#include "MFEPlatform.hpp"
#include "device/AxisPositionMonitor.hpp"
#include "platform/specialized/MFEPlatform/MFEAxis.hpp"
 
using namespace smc;
using namespace smc::internal;
using ImmediateCmd = MGrblPlatform::ImmediateCmd;
using RegInfo = MFEPlatform::RegInfo;
 
MFEAxisPositionMonitor::MFEAxisPositionMonitor(
    const std::string &uid,
    const std::shared_ptr<Axis> &axis,
    const std::shared_ptr<PositionSensor> &position,
    const std::shared_ptr<MFEPlatform> &mfe) :
    AxisPositionMonitor{uid, axis, position},
    m_mfe{mfe}
{}
 
static inline std::future<units::value_t> getReg(MFEPlatform::Shared &mfe,
                                                 const std::string &regName)
{
    return mfe->run<units::value_t>(
        [regName](ImmediateCmd &cmd, const MGrblPlatform::Context &mgrblCtx) {
            const MFEPlatform::Context &ctx(
                static_cast<const MFEPlatform::Context &>(mgrblCtx));
 
            RegInfo info;
            if (!ctx.edge)
                throw Exception{ErrorCode::InvalidArguments,
                                "edge driver not available"};
            else if (!ctx.getRegInfo(regName, info))
                throw Exception{ErrorCode::InvalidArguments,
                                "failed to get reg info: " + regName};
 
            uint32_t value;
            ctx.edge->read(info.reg, info.shift, value);
            units::value_t ret{
                units::steps,
                units::value_t::value_type(reinterpret_cast<int32_t &>(value))};
            cmd.prom->get<units::value_t>().set_value(ret);
        });
}
 
std::future<units::value_t> MFEAxisPositionMonitor::getDifference(
    units::unit_t unit) const
{
    try
    {
        if (unit != units::steps)
            throw Exception{ErrorCode::IncompatibleUnits,
                            "failed to convert to " + to_string(unit)};
 
        MFEPlatform::Shared mfe(m_mfe.lock());
        if (!mfe)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        const std::string regName{uid() + "/diff"};
        return getReg(mfe, regName);
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<units::value_t>(ex);
    }
}
 
std::future<units::value_t> MFEAxisPositionMonitor::getMaxDifference(
    units::unit_t unit) const
{
    try
    {
        if (unit != units::steps)
            throw Exception{ErrorCode::IncompatibleUnits,
                            "failed to convert to " + to_string(unit)};
 
        MFEPlatform::Shared mfe(m_mfe.lock());
        if (!mfe)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        const std::string regName{uid() + "/diff/max"};
        return getReg(mfe, regName);
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<units::value_t>(ex);
    }
}
 
std::future<AxisPositionMonitor::State> MFEAxisPositionMonitor::getState() const
{
    try
    {
        MFEPlatform::Shared mfe(m_mfe.lock());
        std::shared_ptr<const MFEAxisPositionMonitor> self{
            std::static_pointer_cast<const MFEAxisPositionMonitor>(
                shared_from_this())};
 
        if (!mfe)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        return mfe->run<State>(
            [self, mfe](ImmediateCmd &cmd, MGrblPlatform::Context &ctx) {
                mfe->processUpdate(ctx);
 
                cmd.prom->get<State>().set_value(self->lastState());
            });
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<State>(ex);
    }
}
 
std::future<void> MFEAxisPositionMonitor::setEnabled(bool enabled)
{
    try
    {
        MFEPlatform::Shared mfe(m_mfe.lock());
        if (!mfe)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        const std::string regName{uid() + "/slost/gcr"};
        return mfe->run<void>(
            [regName, enabled](ImmediateCmd &cmd,
                               const MGrblPlatform::Context &mgrblCtx) {
                const MFEPlatform::Context &ctx(
                    static_cast<const MFEPlatform::Context &>(mgrblCtx));
 
                RegInfo info;
                if (!ctx.edge)
                    throw Exception{ErrorCode::InvalidArguments,
                                    "edge driver not available"};
                else if (!ctx.getRegInfo(regName, info))
                    throw Exception{ErrorCode::InvalidArguments,
                                    "failed to get reg info: " + regName};
                uint32_t gcr;
                uint32_t mask{info.mask << info.shift};
                ctx.edge->read(info.reg, gcr);
 
                // if already enabled, disable it to reset internal state
                if (!enabled || !(gcr & mask))
                    ctx.edge->write(info.reg, gcr | mask);
                if (enabled)
                    ctx.edge->write(info.reg, gcr & ~mask);
 
                logger::info(MFEPlatform::s_loggerCat)
                    << "step-lost detection "
                    << (enabled ? "enabled" : "disabled") << " on " << regName;
                cmd.prom->get<void>().set_value();
            });
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<void>(ex);
    }
}
 
units::value_t MFEAxisPositionMonitor::lastDifference(units::unit_t unit) const
{
    MFEPlatform::Shared mfe(m_mfe.lock());
    if (!mfe)
        throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
    else if (unit != units::steps)
        throw Exception(ErrorCode::IncompatibleUnits,
                        "failed to convert position");
 
    const std::string &uid{this->uid()};
    uint32_t diff;
    mfe->getResolverMonitorState(
        uid.substr(
            0, uid.length() - MFEPlatform::s_resolverMonitorSuffix.length()),
        diff);
    return units::value_t{units::steps,
                          double(*reinterpret_cast<const int32_t *>(&diff))};
}
 
AxisPositionMonitor::State MFEAxisPositionMonitor::lastState() const
{
    MFEPlatform::Shared mfe(m_mfe.lock());
    if (!mfe)
        throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
    const std::string &uid{this->uid()};
    uint32_t diff;
    return mfe->getResolverMonitorState(
        uid.substr(
            0, uid.length() - MFEPlatform::s_resolverMonitorSuffix.length()),
        diff);
}
 
std::future<std::set<std::string>> MFEAxisPositionMonitor::listConfig() const
{
    try
    {
        std::shared_ptr<MFEPlatform> mfe(m_mfe.lock());
        if (!mfe)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        grbl::Axis grblAxis = mfe->getResolverMonitorAxis(uid());
 
        return mfe->run<std::set<std::string>>(
            [grblAxis, mfe](ImmediateCmd &cmd, MGrblPlatform::Context &ctx) {
                std::set<std::string> ret;
                mfe->runOnInstance(
                    ctx,
                    [&ret, grblAxis](GrblPlatform::Shared &,
                                     GrblPlatform::Context &ctx) {
                        for (const std::string &name : ctx.listConfig(grblAxis))
                        {
                            if (ccut::startsWith(
                                    name, MFEPlatform::s_resolverSettingPrefix))
                            {
                                const std::string setting{name.substr(
                                    MFEPlatform::s_resolverSettingPrefix.size())};
                                if (!MFEPlatform::s_monitorConfigHidden.count(
                                        setting))
                                    ret.insert(setting);
                            }
                        }
                    });
 
                cmd.prom->get<std::set<std::string>>().set_value(ret);
            });
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<std::set<std::string>>(ex);
    }
}
 
std::future<std::string> MFEAxisPositionMonitor::getConfig(
    const std::string &name) const
{
    try
    {
        PositionSensor::Shared position{positionSensorDevice()};
 
        if (!position)
            throw Exception(
                ErrorCode::InvalidArguments,
                "Failed to find positionSensor for device: " + uid());
 
        return position->getConfig(name);
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<std::string>(ex);
    }
}
 
std::future<void> MFEAxisPositionMonitor::setConfig(const std::string &name,
                                                    const std::string &value) const
{
    try
    {
        PositionSensor::Shared position{positionSensorDevice()};
 
        if (!position)
            throw Exception(
                ErrorCode::InvalidArguments,
                "Failed to find positionSensor for device: " + uid());
 
        return position->setConfig(name, value);
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error(ex);
    }
}