MFEPlatform.cpp

/*
** Copyright (C) 2022 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: 2022-11-22T14:52:14
** Author: Sylvain Fargier <sylvain.fargier@cern.ch>
*/
 
#include "MFEPlatform.hpp"
 
#include <memory>
#include <mutex>
 
#include <ccut/async.hpp>
#include <fmc_mfe/libfmc_mfe.h>
 
#include "MFEAxis.hpp"
#include "MFEAxisPositionMonitor.hpp"
#include "MFEEdgeGpio.hpp"
#include "MFEPlatformData.hpp"
#include "MFEPositionSensor.hpp"
#include "Units.hpp"
#include "device/DeviceBase.hpp"
#include "platform/PlatformBase.hpp"
#include "platform/specialized/MGrblPlatform/MGrblPlatform.hpp"
#include "util/edge/EdgeGrblDeviceBase.hpp"
 
// FIXME may get dynamic in build-info
#define MFE_RESOLVER_COUNT 8
 
using namespace logger;
using namespace std::chrono;
 
namespace smc {
namespace internal {
 
static const std::string s_mfePrefix{"mfe://"};
const std::string MFEPlatform::s_resolverMonitorSuffix{"/monitor"};
const std::string MFEPlatform::s_loggerCat{"smc:platform:mfe"};
const std::string s_modeSettingName{"mode"};
const std::string MFEPlatform::s_resolverSettingPrefix{"resolver "};
const std::string MFEPlatform::s_stepLostConfig{"step-lost threshold"};
const std::set<std::string> MFEPlatform::s_resolverConfigHidden{
    "step-lost threshold", "enable", "position"};
const std::set<std::string> MFEPlatform::s_monitorConfigHidden{
    "position", "mode", "enable"};
 
MFEPlatform::MFEPlatform(const ccut::yml::NodeRef &ref) :
    MGrblPlatform{ref, new Context(ref)}
{
    Context &ctx(static_cast<Context &>(*m_ctx));
 
    ctx.edge = std::dynamic_pointer_cast<edge::EdgeGrblDeviceBase>(ctx.device);
}
 
MFEPlatform::~MFEPlatform() {}
 
MGrblAxis::Shared MFEPlatform::createAxis(
    const std::string &uid,
    const std::shared_ptr<GrblPlatform> &grbl)
{
    return std::make_shared<MFEAxis>(uid, shared_from_this(), grbl);
}
 
PlatformBase::DeviceTypeList MFEPlatform::getSupportedDevices() const
{
    return DeviceTypeList{DeviceType::AXIS,
                          DeviceType::TRIGGER,
                          DeviceType::GPIO,
                          DeviceType::PLATFORM,
                          DeviceType::POSITION_SENSOR,
                          DeviceType::AXIS_POSITION_MONITOR};
}
 
PlatformBase::DeviceList MFEPlatform::generateDevices(
    const DeviceTypeList &deviceType)
{
    PlatformBase::DeviceList ret{MGrblPlatform::generateDevices(deviceType)};
 
    if (deviceType.count(DeviceType::GPIO))
    {
        std::shared_ptr<MFEPlatform> self(
            std::static_pointer_cast<MFEPlatform>(shared_from_this()));
 
        std::future<std::list<DeviceBase::Shared>> f =
            run<std::list<DeviceBase::Shared>>([self](ImmediateCmd &cmd,
                                                      MGrblPlatform::Context &) {
                Context &ctx(static_cast<Context &>(*self->m_ctx));
                if (!ctx.edge)
                    throw Exception(ErrorCode::Runtime,
                                    "edge driver not available");
 
                std::list<DeviceBase::Shared> ret;
                self->generateGpios(ctx, ret);
                cmd.prom->get<std::list<DeviceBase::Shared>>().set_value(ret);
            });
        if (f.wait_for(milliseconds(3000)) != std::future_status::ready)
        {
            error(s_loggerCat) << "failed to generate Gpios";
        }
        else
        {
            std::list<DeviceBase::Shared> gpiosList{f.get()};
            ret.insert(ret.end(), gpiosList.begin(), gpiosList.end());
        }
    }
 
    const bool hasAxisPositionMonitor = deviceType.count(
        DeviceType::AXIS_POSITION_MONITOR);
    if (deviceType.count(DeviceType::POSITION_SENSOR))
    {
        std::shared_ptr<MFEPlatform> self(
            std::static_pointer_cast<MFEPlatform>(shared_from_this()));
 
        const std::string platformUrl = m_ctx->prefix + m_ctx->id;
 
        for (size_t i = 0; i < MFE_RESOLVER_COUNT; ++i)
        {
            const grbl::Axis grblAxis = static_cast<grbl::Axis>(i);
            MFEPositionSensor::Shared pos = std::make_shared<MFEPositionSensor>(
                platformUrl + "/resolver/" + grbl::to_string(grblAxis), self);
            MFEAxisPositionMonitor::Shared mon;
 
            if (hasAxisPositionMonitor && deviceType.count(DeviceType::AXIS))
            {
                const std::string axisUid = platformUrl + "/axis/" +
                    grbl::to_string(grblAxis);
                const auto it = std::find_if(
                    ret.begin(), ret.end(),
                    [&axisUid](const DeviceBase::Shared &device) {
                        return device->uid() == axisUid &&
                            device->type() == DeviceType::AXIS;
                    });
                if (it != ret.end())
                {
                    mon = std::make_shared<MFEAxisPositionMonitor>(
                        platformUrl + "/resolver/" + grbl::to_string(grblAxis) +
                            s_resolverMonitorSuffix,
                        std::dynamic_pointer_cast<Axis>(*it), pos, self);
                }
                else
                {
                    error(s_loggerCat)
                        << "Failed to find axis for resolver: "
                        << ret.back()->uid() << ", axis: " << axisUid;
                }
            }
            const std::string uid{pos->uid()};
            std::future<void> f = run<void>([self, grblAxis, i,
                                             uid](ImmediateCmd &cmd,
                                                  MGrblPlatform::Context &) {
                Context &ctx(static_cast<Context &>(*self->m_ctx));
                if (!ctx.edge)
                    throw Exception(ErrorCode::Runtime,
                                    "edge driver not available");
 
                grbl::SettingId modeSettingId = -1;
                self->runOnInstance(
                    ctx,
                    [&modeSettingId, grblAxis](GrblPlatform::Shared &,
                                               GrblPlatform::Context &ctx) {
                        modeSettingId = ctx.findSetting(
                                               "resolver " + s_modeSettingName,
                                               grblAxis)
                                            .id;
                    },
                    true);
                {
                    std::lock_guard<std::mutex> guard{self->m_lock};
                    self->m_resolvers.emplace(
                        uid,
                        ResolverInfo{
                            grblAxis, modeSettingId,
                            units::value_t{units::steps_per_turn, -1.0}});
                }
                ctx.registers[uid + "/" + to_string(units::radians)] = RegInfo{
                    ctx.edge->getDrv()->reg_map->motion_core.res_angle, i, 0,
                    32};
                ctx.registers[uid + "/" + to_string(units::steps)] = RegInfo{
                    ctx.edge->getDrv()->reg_map->motion_core.res_step, i, 0, 32};
                ctx.registers[uid + s_resolverMonitorSuffix + "/diff"] = RegInfo{
                    ctx.edge->getDrv()->reg_map->motion_core.res_drv_diff, i, 0,
                    32};
                ctx.registers[uid + s_resolverMonitorSuffix +
                              "/diff/max"] = RegInfo{
                    ctx.edge->getDrv()->reg_map->motion_core.res_drv_diff_max,
                    i, 0, 32};
                ctx.registers[uid + s_resolverMonitorSuffix + "/slost/gcr"] =
                    RegInfo{ctx.edge->getDrv()->reg_map->motion_core.res_gcr,
                            24, uint32_t(1 << i), 32};
                ctx.registers[uid + s_resolverMonitorSuffix + "/slost/gsr"] =
                    RegInfo{ctx.edge->getDrv()->reg_map->motion_core.res_gsr, 0,
                            uint32_t(1 << i), 32};
                cmd.prom->get<void>().set_value();
            });
            if (f.wait_for(milliseconds(3000)) != std::future_status::ready)
            {
                error(s_loggerCat)
                    << "Failed to create registers for position sensor: "
                    << platformUrl + "/resolver/" + grbl::to_string(grblAxis);
                pos.reset();
                mon.reset();
            }
            /* load parts from config */
            if (pos)
            {
                pos->getConfig(s_modeSettingName).wait();
                ret.push_back(pos);
            }
            if (mon)
                ret.push_back(mon);
        }
    }
 
    if (deviceType.count(DeviceType::AXIS_POSITION_MONITOR))
    {
        if (!deviceType.count(DeviceType::AXIS))
            error(s_loggerCat) << "Can't probe for AxisPositionMonitor objects "
                                  "when Axis are not probed";
        else if (!deviceType.count(DeviceType::POSITION_SENSOR))
            error(s_loggerCat) << "Can't probe for AxisPositionMonitor objects "
                                  "when Position are not probed";
    }
    return ret;
}
 
void MFEPlatform::generateGpios(Context &ctx, std::list<DeviceBase::Shared> &ret)
{
    const std::string platformUrl = ctx.prefix + ctx.id + "/gpio";
 
    ret.emplace_back(std::make_shared<MFEEdgeGpio>(platformUrl + "/dinput",
                                                   shared_from_this()));
    ctx.registers[ret.back()->uid()] = RegInfo{
        ctx.edge->getDrv()->reg_map->motion_core.dinr, 0, 0, 32};
 
    ret.emplace_back(std::make_shared<MFEEdgeGpio>(platformUrl + "/limit",
                                                   shared_from_this()));
    ctx.registers[ret.back()->uid()] = RegInfo{
        ctx.edge->getDrv()->reg_map->grbl_gpio.lir, 0, 0, 32};
 
    ret.emplace_back(std::make_shared<MFEEdgeGpio>(
        platformUrl + "/driver/ready", shared_from_this()));
    ctx.registers[ret.back()->uid()] = RegInfo{
        ctx.edge->getDrv()->reg_map->motion_core.dinr, 18, 0, 8};
 
    ret.emplace_back(std::make_shared<MFEEdgeGpio>(
        platformUrl + "/driver/enable", shared_from_this()));
    ctx.registers[ret.back()->uid()] = RegInfo{
        ctx.edge->getDrv()->reg_map->grbl_gpio.en, 0, 0, 8};
 
    size_t axisCount;
    runOnInstance(
        ctx,
        [&axisCount](GrblPlatform::Shared &, const GrblPlatform::Context &ctx) {
            axisCount = ctx.binfo.axisCount;
        },
        true);
 
    for (size_t i = 0; i < axisCount; ++i)
    {
        const grbl::Axis grblAxis = static_cast<grbl::Axis>(i);
        ret.emplace_back(std::make_shared<MFEEdgeGpio>(
            platformUrl + "/limit/" + grbl::to_string(grblAxis) + "Max",
            shared_from_this()));
        ctx.registers[ret.back()->uid()] = RegInfo{
            ctx.edge->getDrv()->reg_map->grbl_gpio.lir, i, 0x01, 1};
 
        ret.emplace_back(std::make_shared<MFEEdgeGpio>(
            platformUrl + "/limit/" + grbl::to_string(grblAxis) + "Min",
            shared_from_this()));
        ctx.registers[ret.back()->uid()] = RegInfo{
            ctx.edge->getDrv()->reg_map->grbl_gpio.lir, i + 8, 0x01, 1};
 
        ret.emplace_back(std::make_shared<MFEEdgeGpio>(
            platformUrl + "/driver/ready/" + grbl::to_string(grblAxis),
            shared_from_this()));
        ctx.registers[ret.back()->uid()] = RegInfo{
            ctx.edge->getDrv()->reg_map->motion_core.dinr, i + 18, 0x01, 1};
 
        ret.emplace_back(std::make_shared<MFEEdgeGpio>(
            platformUrl + "/driver/enable/" + grbl::to_string(grblAxis),
            shared_from_this()));
        ctx.registers[ret.back()->uid()] = RegInfo{
            ctx.edge->getDrv()->reg_map->grbl_gpio.en, i, 0x01, 1};
    }
}
 
void MFEPlatform::processUpdate(MGrblPlatform::Context &)
{
    Context &ctx(static_cast<Context &>(*m_ctx));
    if (!ctx.edge)
    {
        warning(s_loggerCat)
            << "edge device not available, not updating devices";
        return;
    }
 
    try
    {
        std::set<DeviceId> notifications;
        size_t resolverCount;
        {
            std::lock_guard<std::mutex> guard(m_mutex);
            resolverCount = m_resolvers.size();
        }
 
        if (resolverCount)
        {
            debug(s_loggerCat) << "updating resolvers";
            std::vector<uint32_t> angles(resolverCount, 0);
            std::vector<uint32_t> steps(resolverCount, 0);
            std::vector<uint32_t> diff(resolverCount, 0);
            uint32_t resGCR;
            uint32_t resGSR;
 
            ctx.edge->read(ctx.edge->getDrv()->reg_map->motion_core.res_angle,
                           angles);
            ctx.edge->read(ctx.edge->getDrv()->reg_map->motion_core.res_step,
                           steps);
            ctx.edge->read(
                ctx.edge->getDrv()->reg_map->motion_core.res_drv_diff, diff);
            ctx.edge->read(ctx.edge->getDrv()->reg_map->motion_core.res_gsr,
                           resGSR);
            ctx.edge->read(ctx.edge->getDrv()->reg_map->motion_core.res_gcr,
                           resGCR);
 
            {
                std::lock_guard<std::mutex> guard(m_mutex);
                for (auto &it : m_resolvers)
                {
                    size_t index = size_t(it.second.grblAxis);
                    if (index >= resolverCount)
                    {
                        crit(s_loggerCat)
                            << "invalid index for resolver: " << index;
                        continue;
                    }
                    AxisPositionMonitor::State state;
                    const uint8_t axis_bit = 1 << index;
                    if (resGCR & (axis_bit << 24))
                        state = AxisPositionMonitor::State::DISABLED;
                    else if (resGSR & axis_bit)
                        state = AxisPositionMonitor::State::TRIGGERED;
                    else
                        state = AxisPositionMonitor::State::ENABLED;
 
                    if (steps[index] != it.second.stepReg)
                        notifications.insert(it.first);
                    it.second.stepReg = steps[index];
                    it.second.angleReg = angles[index];
 
                    if (state != it.second.monitorState)
                    {
                        info(s_loggerCat)
                            << "resolver step-lost state changed: " << it.first
                            << " is " << state;
                        notifications.insert(it.first + s_resolverMonitorSuffix);
                    }
                    it.second.monitorState = state;
                    it.second.diffReg = diff[index];
                }
            }
        }
 
        /* send notifications */
        notify(std::move(notifications));
    }
    catch (const std::exception &ex)
    {
        error(s_loggerCat) << "MFEPlatform error: " << ex.what();
    }
 
    MGrblPlatform::processUpdate(ctx);
}
 
MFEPlatform::Context::Context(const ccut::yml::NodeRef &ref) :
    MGrblPlatform::Context("mfe://", ref)
{}
 
bool MFEPlatform::Context::getRegInfo(const std::string &uid,
                                      RegInfo &info) const
{
    std::map<std::string, RegInfo>::const_iterator it{registers.find(uid)};
 
    if (it == registers.cend())
        return false;
 
    info = it->second;
    return true;
}
 
const MFEPlatform::ResolverInfo &MFEPlatform::_getResolverInfo(
    const std::string &uid) const
{
    std::map<std::string, ResolverInfo>::const_iterator it{
        m_resolvers.find(uid)};
    if (it == m_resolvers.cend())
        throw Exception(ErrorCode::InvalidArguments,
                        "invalid position object: " + uid);
    else
        return it->second;
}
 
grbl::Axis MFEPlatform::getResolverAxis(const std::string &uid) const
{
    std::lock_guard<std::mutex> l(m_lock);
    return _getResolverInfo(uid).grblAxis;
}
 
grbl::Axis MFEPlatform::getResolverMonitorAxis(const std::string &uid) const
{
    const std::string resolverUid = uid.substr(
        0, uid.length() - s_resolverMonitorSuffix.length());
    return getResolverAxis(resolverUid);
}
 
void MFEPlatform::updateResolverSetting(const std::string &uid,
                                        grbl::SettingId settingId,
                                        const std::string &value)
{
    std::unique_lock<std::mutex> l(m_lock);
    std::map<std::string, ResolverInfo>::iterator it{m_resolvers.find(uid)};
    if (it == m_resolvers.cend())
        throw Exception(ErrorCode::InvalidArguments,
                        "invalid position object: " + uid);
 
    if (settingId == it->second.modeSettingId)
    {
        it->second.mode.value = std::stof(value);
        l.unlock();
 
        info(s_loggerCat) << "updating resolver " << uid << " mode: " << value;
    }
}
 
units::value_t MFEPlatform::getResolverPosition(const std::string &uid,
                                                const units::unit_t unit) const
{
    std::lock_guard<std::mutex> l(m_lock);
    switch (unit)
    {
    case units::steps:
    {
        return units::value_t{unit,
                              double(*reinterpret_cast<const int32_t *>(
                                  &_getResolverInfo(uid).stepReg))};
    }
    case units::radians:
    {
        return units::value_t{
            unit,
            *reinterpret_cast<const float *>(reinterpret_cast<const void *>(
                &_getResolverInfo(uid).angleReg))};
    }
    default:
        throw Exception(ErrorCode::IncompatibleUnits,
                        "failed to convert position");
    }
}
 
units::value_t MFEPlatform::getResolverMode(const std::string &uid) const
{
    std::lock_guard<std::mutex> l(m_lock);
    return _getResolverInfo(uid).mode;
}
 
MFEAxisPositionMonitor::State MFEPlatform::getResolverMonitorState(
    const std::string &uid,
    uint32_t &diff)
{
    std::lock_guard<std::mutex> l(m_lock);
    const ResolverInfo &info = _getResolverInfo(uid);
    diff = info.diffReg;
    return info.monitorState;
}
 
} // namespace internal
} // namespace smc