GrblPlatform.cpp

/*
** Copyright (C) 2021 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-02-11T15:44:13+01:00
**     Author: Sylvain Fargier <sfargier> <sylvain.fargier@cern.ch>
**
*/
 
#include "GrblPlatform.hpp"
 
#include <algorithm>
#include <cerrno>
#include <chrono>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <exception>
 
#include <ccut/Regex.hpp>
#include <ccut/async.hpp>
#include <ccut/utils.hpp>
#include <fcntl.h>
#include <logger/Logger.hpp>
#include <poll.h>
#include <unistd.h>
 
#include "GrblPlatformData.hpp"
#include "MotionLibConfig.hpp"
#include "util/Promise.hpp"
#include "util/grbl/GrblGenerator.hpp"
#include "util/grbl/GrblParser.hpp"
#include "util/grbl/GrblSim.hpp"
#include "util/grbl/GrblTypes.hpp"
 
#ifdef PLATFORM_SAMBUCA
#include "util/edge/EdgeGrblDevice.hpp"
#endif
 
using namespace logger;
using namespace std::chrono;
namespace yml = ccut::yml;
using grbl::GrblParser;
 
#define GRBL_MIN_POS_CHANGE 0.001
 
static const ccut::Regex s_errorCode("^\\[ERRORCODE:([[:digit:]]+)\\|\\|(.+)"
                                     "\\]");
static const ccut::Regex s_alarmCode("^\\[ALARMCODE:([[:digit:]]+)\\|\\|(.+)"
                                     "\\]");
 
namespace smc {
namespace internal {
 
const std::string GrblPlatform::s_loggerCat{"smc:platform:grbl"};
 
const milliseconds GrblPlatform::defaultInterval(1000);
 
GrblPlatform::GrblPlatform(const ccut::yml::NodeRef &ref) :
    Thread("GrblPlatform")
{
    std::string type;
    std::string id;
    yml::NodeRef deviceNode = yml::get(ref, "device");
    yml::get(ref, "id") >> yml::default_to(id, "0");
 
    m_ctx.reset(new GrblPlatform::Context("grbl://", id));
 
    if (deviceNode.is_map())
        yml::get(deviceNode, "type") >> yml::default_to(type, std::string());
    else
        deviceNode >> yml::default_to(type, std::string());
 
    if (type == "GrblSim")
    {
        m_ctx->device.reset(new grbl::GrblSim());
    }
#ifdef PLATFORM_SAMBUCA
    else if (type == "EdgeSim")
    {
        int lun;
        std::string path;
        yml::get(deviceNode, "lun") >> yml::default_to(lun, int());
        yml::get(deviceNode, "path") >> yml::default_to(path, std::string());
        m_ctx->device.reset(new edge::EdgeGrblDevice(lun, path));
    }
    else if (type == "Edge")
    {
        int lun;
        yml::get(deviceNode, "lun") >> yml::default_to(lun, int());
        m_ctx->device.reset(new edge::EdgeGrblDevice(lun));
    }
#endif /* PLATFORM_SAMBUCA */
    else
        throw Exception(ErrorCode::InvalidArguments,
                        "unknown GrblPlatform device type");
 
    m_ctx->device->dataSignal.connect(
        m_dataConn, [this](const std::string &line) { processMessage(line); });
 
    start();
}
 
GrblPlatform::GrblPlatform(const std::string &prefix,
                           const std::string &id,
                           std::unique_ptr<grbl::GrblDeviceBase> &&device) :
    Thread("GrblPlatform"),
    m_ctx(new GrblPlatform::Context(prefix, id))
{
    m_ctx->device.swap(device);
    m_ctx->device->dataSignal.connect(
        m_dataConn, [this](const std::string &line) { processMessage(line); });
}
 
GrblPlatform::~GrblPlatform()
{
    GrblPlatform::stop();
    m_dataConn.disconnect();
    m_ctx->device.reset();
}
 
GrblPlatform::AxisData &GrblPlatform::_getAxisData(const std::string &uid)
{
    decltype(m_axisMap)::iterator it = m_axisMap.find(uid);
    if (it == m_axisMap.end())
    {
        error(GrblPlatform::s_loggerCat) << "invalid uid for platform: " << uid;
        throw Exception(ErrorCode::InvalidArguments,
                        "invalid uid for platform: " + uid);
    }
    return it->second;
}
 
grbl::Axis GrblPlatform::getAxis(const std::string &uid)
{
    std::lock_guard<std::mutex> l(m_lock);
    return _getAxisData(uid).grblAxis;
}
 
void GrblPlatform::updateAxisSetting(const std::string &axis,
                                     grbl::SettingId settingId,
                                     const std::string &value)
{
    std::lock_guard<std::mutex> l(m_lock);
    AxisData &data = _getAxisData(axis);
    if (settingId == data.resolutionSettingId)
    {
        info(s_loggerCat)
            << "updating axis " << axis << " resolution: " << value << " "
            << to_string(data.resolution.unit);
        data.resolution.value = std::stof(value);
    }
}
 
units::Value GrblPlatform::getLastAxisPosition(const std::string &axis) const
{
    std::lock_guard<std::mutex> l(m_lock);
    decltype(GrblPlatform::m_axisMap)::const_iterator it = m_axisMap.find(axis);
    if (it == m_axisMap.end())
    {
        error(s_loggerCat) << "invalid uid for platform: " << axis;
        throw Exception(ErrorCode::InvalidArguments,
                        "invalid uid for platform: " + axis);
    }
    return it->second.position;
}
 
units::Value GrblPlatform::getLastAxisResolution(const std::string &axis) const
{
    std::lock_guard<std::mutex> l(m_lock);
    decltype(GrblPlatform::m_axisMap)::const_iterator it = m_axisMap.find(axis);
    if (it == m_axisMap.end())
    {
        error(s_loggerCat) << "invalid uid for platform: " << axis;
        throw Exception(ErrorCode::InvalidArguments,
                        "invalid uid for platform: " + axis);
    }
    return it->second.resolution;
}
 
uint8_t GrblPlatform::getTrigger(const std::string &uid)
{
    if (!ccut::startsWith(uid, m_trigger.first))
    {
        error(s_loggerCat) << "invalid uid for platform: " << uid;
        throw Exception(ErrorCode::InvalidArguments,
                        "invalid uid for platform: " + uid);
    }
    return m_trigger.second;
}
 
uint8_t GrblPlatform::getGpio(const std::string &uid)
{
    std::lock_guard<std::mutex> l(m_lock);
    decltype(m_gpioMap)::const_iterator it = std::find_if(
        m_gpioMap.begin(), m_gpioMap.end(),
        [&uid](const std::pair<std::string, uint8_t> &p) {
            return ccut::startsWith(uid, p.first);
        });
    if (it == m_gpioMap.end())
    {
        error(s_loggerCat) << "invalid uid for platform: " << uid;
        throw Exception(ErrorCode::InvalidArguments,
                        "invalid uid for platform: " + uid);
    }
    return it->second;
}
 
void GrblPlatform::clearQueues(const std::string &msg, bool clearImmediates)
{
    info(s_loggerCat) << "clearing run-queues";
    std::unique_lock<std::mutex> lock(m_lock);
    if (clearImmediates)
    {
        while (!m_immediate.empty())
        {
            ImmediateCmd cmd = std::move(m_immediate.front());
            m_immediate.pop();
            cmd.prom->set_exception(Exception(ErrorCode::Canceled, msg));
        }
    }
    while (!m_queue.empty())
    {
        MotionCmd cmd = std::move(m_queue.front());
        m_queue.pop();
        cmd.prom.set_exception(
            std::make_exception_ptr(Exception(ErrorCode::Canceled, msg)));
    }
    while (!m_pending.empty())
    {
        MotionCmd cmd = std::move(m_pending.front());
        m_pending.pop();
        cmd.prom.set_exception(
            std::make_exception_ptr(Exception(ErrorCode::Runtime, msg)));
    }
}
 
void GrblPlatform::wake()
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_waked = true;
    if (m_ctx->device)
        m_ctx->device->wake();
}
 
void GrblPlatform::stop()
{
    Thread::stop();
}
 
void GrblPlatform::processMessage(const std::string &message)
{
    try
    {
        bool handled = false;
        switch (GrblParser::getLineType(message))
        {
        case grbl::LineType::State:
            if (GrblParser::parseState(message, m_ctx->state))
            {
                onStateMessage();
                handled = true;
            }
            break;
        case grbl::LineType::Message:
            if (GrblPlatform::Context::isStepperMessage(message))
            {
                m_ctx->nextPoll = steady_clock::now();
                handled = true;
            }
            break;
        default: break;
        }
 
        if (!handled)
        {
            debug(s_loggerCat) << "dropping message: " << message;
        }
    }
    catch (std::exception &ex)
    {
        error(s_loggerCat) << "failed to processMessage: " << ex.what();
    }
}
 
std::future<void> GrblPlatform::queue(MotionCmd &&motion)
{
    std::future<void> ret = motion.prom.get_future();
    std::unique_lock<std::mutex> lock(m_lock);
    if (m_started)
    {
        m_queue.push(std::move(motion));
        lock.unlock();
        wake();
    }
    else
        motion.prom.set_exception(std::make_exception_ptr(
            Exception(ErrorCode::Canceled, "platform stopped")));
    return ret;
}
 
void GrblPlatform::thread_func()
{
    debug(s_loggerCat) << "started";
    m_ctx->nextPoll = steady_clock::now();
    const milliseconds interval = defaultInterval;
 
    try
    {
        loadErrors();
        loadAlarms();
        loadSettingsDesc();
        // Kill any ongoing actions
        m_ctx->sendReset();
        // test in hard limits and homing
        // make error grbl level then start server obj
        setup();
 
        while (m_started.load())
        {
            steady_clock::time_point now = steady_clock::now();
            const milliseconds timeout = (now < m_ctx->nextPoll) ?
                duration_cast<milliseconds>(m_ctx->nextPoll - now) :
                milliseconds::zero();
 
            m_ctx->device->fetch(timeout);
 
            this->handleAlarm();
 
            while (processImmediate())
                ;
 
            processMotion();
 
            now = steady_clock::now();
            if (m_ctx->nextPoll <= now)
            {
                m_ctx->device->sendRealtime("?");
                m_ctx->nextPoll = now + interval;
            }
        }
    }
    catch (Exception &ex)
    {
        crit(s_loggerCat) << ex.what();
        crit(s_loggerCat) << "stopping thread";
        m_started.store(false);
    }
    clearQueues("platform stopped");
}
 
void GrblPlatform::loadErrors()
{
    /* process errors list */
    m_ctx->checkReply(m_ctx->device->send(
        grbl::gen::System::EnumerateErrors, [this](const std::string &line) {
            std::vector<std::string> matches(3);
            if (s_errorCode.match(line, matches))
            {
                int code = std::atoi(matches[1].c_str());
                if (code == 0)
                    error(s_loggerCat) << "invalid error-code: " << line;
                else
                    m_ctx->errors[code] = matches[2];
                return true;
            }
            return false;
        }));
    info(s_loggerCat) << "loaded error-codes: " << m_ctx->errors.size();
    m_ctx->errors[grbl::CustomErrorCode::Timeout] = "request timeout";
    m_ctx->errors[grbl::CustomErrorCode::NoPayload] = "no payload";
    m_ctx->errors[grbl::CustomErrorCode::InternalError] = "internal error";
}
 
void GrblPlatform::loadAlarms()
{
    /* process alarms list */
    m_ctx->checkReply(m_ctx->device->send(
        grbl::gen::System::EnumerateAlarms, [this](const std::string &line) {
            std::vector<std::string> matches(3);
            if (s_alarmCode.match(line, matches))
            {
                int code = std::atoi(matches[1].c_str());
                if (code == 0)
                    error(s_loggerCat) << "invalid alarm-code: " << line;
                else
                    m_ctx->alarms[code] = matches[2];
                return true;
            }
            return false;
        }));
    info(s_loggerCat) << "loaded alarm-codes: " << m_ctx->alarms.size();
}
 
void GrblPlatform::loadSettingsDesc()
{
    /* retrieve settingGroup */
    m_ctx->checkReply(m_ctx->device->send(
        grbl::gen::System::EnumerateGroups, [this](const std::string &line) {
            GrblParser::SettingGroup group;
            if (GrblParser::parseSettingGroup(line, group))
            {
                m_ctx->settingGroup[group.name] = group.id;
                return true;
            }
            return false;
        }));
    info(s_loggerCat) << "loaded settingGroups: " << m_ctx->settingGroup.size();
 
    m_ctx->checkReply(m_ctx->device->send(
        grbl::gen::System::EnumerateSettings,
        [this](const std::string &line) {
            GrblParser::SettingDesc desc;
            if (GrblParser::parseSettingDesc(line, desc))
            {
                m_ctx->setting[desc.name] = desc;
                return true;
            }
            return false;
        },
        std::chrono::milliseconds(5000)));
    info(s_loggerCat) << "loaded settings: " << m_ctx->setting.size();
}
 
void GrblPlatform::setup()
{
    m_ctx->_sendSetSetting(
        m_ctx->findSetting("Status report options").id,
        std::to_string((grbl::gen::Setting::StatusReportBit::BufferState |
                        grbl::gen::Setting::StatusReportBit::LineNumbers |
                        grbl::gen::Setting::StatusReportBit::FeedSpeed |
                        grbl::gen::Setting::StatusReportBit::PinState |
                        grbl::gen::Setting::StatusReportBit::ParserState |
                        grbl::gen::Setting::StatusReportBit::AlarmSubState)
                           .bits()));
    /* enable hard limits */
    m_ctx->_sendSetSetting(m_ctx->findSetting("Hard limits enable").id, "3");
    m_ctx->checkReply(
        m_ctx->device->send(grbl::gen::CoordinateSystem::set(0).value));
    m_ctx->checkReply(m_ctx->device->send(
        grbl::gen::System::BuildInfo, [this](const std::string &line) {
            return GrblParser::parseBuildInfo(line, m_ctx->binfo);
        }));
}
 
bool GrblPlatform::processImmediate()
{
    std::unique_lock<std::mutex> lock(m_lock);
    if (m_immediate.empty())
        return false;
    ImmediateCmd cmd = std::move(m_immediate.front());
    m_immediate.pop();
    lock.unlock();
 
    try
    {
        cmd.run(cmd, *m_ctx);
    }
    catch (Exception &ex)
    {
        cmd.prom->set_exception(ex);
    }
    catch (std::exception &ex)
    {
        cmd.prom->set_exception(Exception(ErrorCode::Runtime, ex.what()));
    }
    return true;
}
 
bool GrblPlatform::processMotion()
{
    /* do not queue motions if Grbl is not ready for it */
    if (m_ctx->state.motionBufferFree <= 0 ||
        (m_ctx->state.runState != grbl::RunState::Idle &&
         m_ctx->state.runState != grbl::RunState::Run))
        return false;
 
    std::unique_lock<std::mutex> lock(m_lock);
    if (m_queue.empty())
        return false;
    MotionCmd cmd = std::move(m_queue.front());
    m_queue.pop();
    lock.unlock();
    try
    {
        cmd.run(cmd, *m_ctx);
    }
    catch (Exception &ex)
    {
        cmd.prom.set_exception(std::make_exception_ptr(ex));
        return false;
    }
    catch (std::exception &ex)
    {
        cmd.prom.set_exception(
            std::make_exception_ptr(Exception(ErrorCode::Runtime, ex.what())));
        return false;
    }
 
    m_pending.push(std::move(cmd));
    --m_ctx->state.motionBufferFree;
    return true;
}
 
void GrblPlatform::onStateMessage()
{
    /* this may be received in a middle of a transaction, so no commands allowed
     */
    if (m_ctx->state.runState == grbl::RunState::Alarm)
    {
        wake();
    }
    else
    {
        /* update positions, prepare notifications */
        std::set<DeviceId> notifications;
        {
            std::lock_guard<std::mutex> lock(m_lock);
            for (auto &it : m_axisMap)
            {
                const auto &newPosition = m_ctx->state.motorPositions.find(
                    it.second.grblAxis);
                if (newPosition == m_ctx->state.motorPositions.end())
                    warning(s_loggerCat) << "no status for axis " << it.first;
                else if (std::isnan(it.second.position.value) ||
                         fabs(it.second.position.value - newPosition->second) >=
                             GRBL_MIN_POS_CHANGE)
                {
                    it.second.position.value = newPosition->second;
                    notifications.insert(it.first);
                }
            };
        }
 
        /* update tasks */
        size_t motionQueued = m_ctx->binfo.motionBufferSize -
            m_ctx->state.motionBufferFree;
        /*
         * block is freed before motion is stopped (last segment remains)
         * let's preserve last queued motion until everything is properly parked.
         */
        if (!motionQueued && (m_ctx->state.runState == grbl::RunState::Run))
            motionQueued = 1;
        while (m_pending.size() > motionQueued)
        {
            m_pending.front().prom.set_value();
            m_pending.pop();
        }
 
        /* send notifications */
        notify(std::move(notifications));
    }
}
 
void GrblPlatform::handleAlarm()
{
    if (m_ctx->state.runState != grbl::RunState::Alarm)
        return;
 
    const std::string msg = m_ctx->getAlarmMessage(
        static_cast<grbl::AlarmCode>(m_ctx->state.runStateArg));
    warning(s_loggerCat) << "alarm detected, resetting: " << msg;
    clearQueues(msg, false);
    info(s_loggerCat) << "motion-queue flushed";
 
    m_ctx->sendReset();
}
 
} // namespace internal
} // namespace smc