MFEPositionSensor.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-17T16:21:03
** Author: Sylvain Fargier <sylvain.fargier@cern.ch>
*/
 
#include "MFEPositionSensor.hpp"
 
#include <ccut/async.hpp>
#include <ccut/utils.hpp>
 
#include "MFEPlatform.hpp"
#include "Units.hpp"
#include "platform/specialized/MGrblPlatform/MGrblPlatform.hpp"
#include "util/grbl/GrblTypes.hpp"
 
using namespace smc;
using namespace smc::internal;
using ImmediateCmd = MGrblPlatform::ImmediateCmd;
using RegInfo = MFEPlatform::RegInfo;
 
MFEPositionSensor::MFEPositionSensor(const std::string &uid,
                                     const std::shared_ptr<MFEPlatform> &mfe) :
    PositionSensor{uid},
    m_mfe{mfe}
{}
 
std::future<units::value_t> MFEPositionSensor::getPosition(
    units::unit_t unit) const
{
    try
    {
        std::shared_ptr<MFEPlatform> mfe(m_mfe.lock());
        if (!mfe)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        const std::string uid{this->uid()};
        return mfe->run<units::value_t>(
            [uid, unit](ImmediateCmd &cmd,
                        const MGrblPlatform::Context &mgrblCtx) {
                const MFEPlatform::Context &ctx(
                    static_cast<const MFEPlatform::Context &>(mgrblCtx));
 
                const std::string regUid = uid + "/" + to_string(unit);
                RegInfo info;
                if (!ctx.edge)
                    throw Exception{ErrorCode::InvalidArguments,
                                    "edge driver not available"};
                else if (!ctx.getRegInfo(regUid, info))
                    throw Exception{ErrorCode::InvalidArguments,
                                    "failed to get reg info: " + regUid};
 
                uint32_t value;
                ctx.edge->read(info.reg, info.shift, value);
 
                switch (unit)
                {
                case units::steps:
 
                    cmd.prom->get<units::value_t>().set_value(units::value_t{
                        unit,
                        units::value_t::value_type(
                            reinterpret_cast<const int32_t &>(value))});
                    break;
 
                case units::radians:
                    cmd.prom->get<units::value_t>().set_value(units::value_t{
                        unit,
                        units::value_t::value_type(
                            reinterpret_cast<const float &>(value))});
                    break;
                default:
                    throw Exception(ErrorCode::IncompatibleUnits,
                                    "failed to convert to " + to_string(unit));
                }
            });
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<units::value_t>(ex);
    }
}
 
std::future<void> MFEPositionSensor::setActualPosition(
    const units::value_t &value) const
{
    try
    {
        units::value_t stepsValue{value};
 
        if (value.unit == units::radians)
            throw Exception(
                ErrorCode::IncompatibleUnits,
                "can't modify resolver readout position (in radians)");
        else if (!convert(stepsValue, units::steps))
            throw Exception(ErrorCode::IncompatibleUnits,
                            "failed to convert in steps");
 
        return setConfig("position", std::to_string(uint32_t(stepsValue.value)));
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<void>(ex);
    }
}
 
units::value_t MFEPositionSensor::lastPosition(const units::unit_t unit) const
{
    std::shared_ptr<MFEPlatform> mfe(m_mfe.lock());
    if (!mfe)
        throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
    return mfe->getResolverPosition(uid(), unit);
}
 
std::future<std::set<std::string>> MFEPositionSensor::listConfig() const
{
    try
    {
        std::shared_ptr<MFEPlatform> mfe(m_mfe.lock());
        if (!mfe)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        grbl::Axis grblAxis = mfe->getResolverAxis(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_resolverConfigHidden.count(
                                        setting))
                                    ret.insert(setting);
                            }
                        }
                    },
                    true);
 
                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> MFEPositionSensor::getConfig(
    const std::string &name) const
{
    try
    {
        std::shared_ptr<MFEPlatform> mfe(m_mfe.lock());
        if (!mfe)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        const std::string uid{this->uid()};
        grbl::Axis grblAxis = mfe->getResolverAxis(uid);
 
        return mfe->run<std::string>(
            [grblAxis, mfe, uid, name](ImmediateCmd &cmd,
                                       MGrblPlatform::Context &ctx) {
                std::string ret;
                mfe->runOnInstance(
                    ctx,
                    [&ret, grblAxis, mfe, uid,
                     name](GrblPlatform::Shared &, GrblPlatform::Context &ctx) {
                        grbl::SettingId id =
                            ctx.findSetting(
                                   MFEPlatform::s_resolverSettingPrefix + name,
                                   grblAxis)
                                .id;
 
                        ret = ctx._sendGetSetting(id);
                        mfe->updateResolverSetting(uid, id, ret);
                    },
                    true);
 
                cmd.prom->get<std::string>().set_value(ret);
            });
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<std::string>(ex);
    }
}
 
std::future<void> MFEPositionSensor::setConfig(const std::string &name,
                                               const std::string &value) const
{
    try
    {
        std::shared_ptr<MFEPlatform> mfe(m_mfe.lock());
        if (!mfe)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        const std::string uid{this->uid()};
        grbl::Axis grblAxis = mfe->getResolverAxis(uid);
 
        return mfe->run<void>([grblAxis, mfe, uid, name,
                               value](ImmediateCmd &cmd,
                                      MGrblPlatform::Context &ctx) {
            mfe->runOnInstance(
                ctx,
                [grblAxis, mfe, uid, name, value](GrblPlatform::Shared &,
                                                  GrblPlatform::Context &ctx) {
                    grbl::SettingId id =
                        ctx.findSetting(
                               MFEPlatform::s_resolverSettingPrefix + name,
                               grblAxis)
                            .id;
 
                    ctx._sendSetSetting(id, value);
                    mfe->updateResolverSetting(uid, id, value);
                },
                false);
 
            cmd.prom->get<void>().set_value();
        });
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error(ex);
    }
}
 
bool MFEPositionSensor::canConvert(const units::value_t &value,
                                   units::unit_t unit) const
{
    units::value_t temp{value};
    return convert(temp, unit);
}
 
static const std::set<units::unit_t> s_angle{
    units::unit_t::degrees, units::unit_t::radians, units::unit_t::turns};
 
bool MFEPositionSensor::convert(units::value_t &value, units::unit_t unit) const
{
    std::shared_ptr<MFEPlatform> mfe(m_mfe.lock());
    if (!mfe)
        throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
    if (unit == value.unit)
        return true;
    else if (value.unit == units::steps && s_angle.count(unit))
    {
        units::value_t mode = mfe->getResolverMode(uid());
        if (mode.value <= 0)
        {
            logger::warning(MFEPlatform::s_loggerCat)
                << "resolver mode not set";
            return false;
        }
        value = value / mode;
        value.convert(unit);
        return true;
    }
    else if (s_angle.count(value.unit) && unit == units::steps)
    {
        units::value_t mode = mfe->getResolverMode(uid());
        if (mode.value <= 0)
        {
            logger::warning(MFEPlatform::s_loggerCat)
                << "resolver mode not set";
            return false;
        }
        value.convert(units::turns);
        value = mode * value;
        return true;
    }
    else
        return convert(value, unit);
}