planewave_efa.cpp

Go to the documentation of this file.

/*
    PlaneWave EFA Protocol
 
    Hendrick Focuser
 
    Copyright (C) 2020 Jasem Mutlaq (mutlaqja@ikarustech.com)
 
    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.
 
    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.
 
    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 
*/
 
#include "planewave_efa.h"
#include "indicom.h"
#include "connectionplugins/connectionserial.h"
 
#include <cmath>
#include <memory>
#include <cstring>
#include <termios.h>
#include <unistd.h>
#include <regex>
#include <sys/ioctl.h>
 
static std::unique_ptr<EFA> steelDrive(new EFA());
 
EFA::EFA()
{
    setVersion(1, 2);
 
    // Focuser Capabilities
    FI::SetCapability(FOCUSER_CAN_ABS_MOVE |
                      FOCUSER_CAN_REL_MOVE |
                      FOCUSER_CAN_ABORT |
                      FOCUSER_CAN_REVERSE |
                      FOCUSER_CAN_SYNC);
}
 
bool EFA::initProperties()
{
    INDI::Focuser::initProperties();
 
    // Focuser Information
    IUFillText(&InfoT[INFO_VERSION], "INFO_VERSION", "Version", "NA");
    IUFillTextVector(&InfoTP, InfoT, 1, getDeviceName(), "INFO", "Info", MAIN_CONTROL_TAB, IP_RO, 60, IPS_IDLE);
 
    // Focuser temperature
    IUFillNumber(&TemperatureN[TEMPERATURE_PRIMARY], "TEMPERATURE_PRIMARY", "Primary (c)", "%.2f", -50, 70., 0., 0.);
    IUFillNumber(&TemperatureN[TEMPERATURE_AMBIENT], "TEMPERATURE_AMBIENT", "Ambient (c)", "%.2f", -50, 70., 0., 0.);
    IUFillNumberVector(&TemperatureNP, TemperatureN, 2, getDeviceName(), "FOCUS_TEMPERATURE", "Temperature",
                       MAIN_CONTROL_TAB, IP_RO, 0, IPS_IDLE);
 
    // Fan Control
    IUFillSwitch(&FanStateS[FAN_ON], "FAN_ON", "On", ISS_OFF);
    IUFillSwitch(&FanStateS[FAN_OFF], "FAN_OFF", "Off", ISS_ON);
    IUFillSwitchVector(&FanStateSP, FanStateS, 2, getDeviceName(), "FOCUS_FAN", "Fans", FAN_TAB, IP_RW, ISR_1OFMANY, 0,
                       IPS_IDLE);
 
    // Fan Control Mode
    IUFillSwitch(&FanControlS[FAN_MANUAL], "FAN_MANUAL", "Manual", ISS_ON);
    IUFillSwitch(&FanControlS[FAN_AUTOMATIC_ABSOLUTE], "FAN_AUTOMATIC_ABSOLUTE", "Auto. Absolute", ISS_OFF);
    IUFillSwitch(&FanControlS[FAN_AUTOMATIC_RELATIVE], "FAN_AUTOMATIC_RELATIVE", "Auto. Relative", ISS_OFF);
    IUFillSwitchVector(&FanControlSP, FanControlS, 3, getDeviceName(), "FOCUS_FAN_CONTROL", "Control Mode", FAN_TAB, IP_RW,
                       ISR_1OFMANY, 0, IPS_IDLE);
 
    // Fan Control Parameters
    IUFillNumber(&FanControlN[FAN_MAX_ABSOLUTE], "FAN_MAX_ABSOLUTE", "Max Primary (c)", "%.2f", 0, 50., 5., 25.);
    IUFillNumber(&FanControlN[FAN_MAX_RELATIVE], "FAN_MAX_RELATIVE", "Max Relative (c)", "%.2f", 0., 30., 1., 2.5);
    IUFillNumber(&FanControlN[FAN_DEADZONE], "FAN_DEADZONE", "Deadzone (c)", "%.2f", 0.1, 10, 0.5, 0.5);
    IUFillNumberVector(&FanControlNP, FanControlN, 3, getDeviceName(), "FOCUS_FAN_PARAMS", "Control Params",
                       FAN_TAB, IP_RW, 0, IPS_IDLE);
 
    // Fan Off on Disconnect
    IUFillSwitch(&FanDisconnectS[FAN_OFF_ON_DISCONNECT], "FAN_OFF_ON_DISCONNECT", "Switch Off", ISS_ON);
    IUFillSwitchVector(&FanDisconnectSP, FanDisconnectS, 1, getDeviceName(), "FOCUS_FAN_DISCONNECT", "On Disconnect", FAN_TAB,
                       IP_RW, ISR_NOFMANY, 0, IPS_IDLE);
 
    // Calibration Control
    IUFillSwitch(&CalibrationStateS[CALIBRATION_ON], "CALIBRATION_ON", "Calibrated", ISS_OFF);
    IUFillSwitch(&CalibrationStateS[CALIBRATION_OFF], "CALIBRATION_OFF", "Not Calibrated", ISS_ON);
    IUFillSwitchVector(&CalibrationStateSP, CalibrationStateS, 2, getDeviceName(), "FOCUS_CALIBRATION", "Calibration",
                       MAIN_CONTROL_TAB, IP_RW, ISR_1OFMANY, 0, IPS_IDLE);
 
    // Setup limits
    FocusMaxPosN[0].value = 1e7;
    FocusMaxPosN[0].max   = 1e7;
    FocusMaxPosN[0].step  = FocusMaxPosN[0].max / 50;
 
    FocusAbsPosN[0].max   = 1e7;
    FocusAbsPosN[0].step  = FocusAbsPosN[0].max / 50;
 
    FocusSyncN[0].max     = 1e7;
    FocusSyncN[0].step    = FocusSyncN[0].max / 50;
 
    FocusRelPosN[0].max   = FocusAbsPosN[0].max / 2;
    FocusRelPosN[0].step  = FocusRelPosN[0].max / 50;
 
    addAuxControls();
    serialConnection->setDefaultBaudRate(Connection::Serial::B_19200);
    setDefaultPollingPeriod(500);
 
    // Lower RTS so serial port not monopolized and hand controller can work
    int bits = TIOCM_RTS;
    (void) ioctl(PortFD, TIOCMBIC, &bits);
 
    return true;
}
 
bool EFA::updateProperties()
{
    INDI::Focuser::updateProperties();
 
    if (isConnected())
    {
        getStartupValues();
 
        defineProperty(&InfoTP);
        defineProperty(&CalibrationStateSP);
 
        // Fan
        defineProperty(&FanStateSP);
        defineProperty(&FanControlSP);
        loadConfig(true, FanControlSP.name);
        defineProperty(&FanDisconnectSP);
 
        defineProperty(&TemperatureNP);
    }
    else
    {
        deleteProperty(InfoTP.name);
        deleteProperty(CalibrationStateSP.name);
 
        deleteProperty(FanStateSP.name);
        deleteProperty(FanControlSP.name);
        deleteProperty(FanControlNP.name);
        deleteProperty(FanDisconnectSP.name);
 
        deleteProperty(TemperatureNP.name);
    }
 
    return true;
}
 
bool EFA::Disconnect()
{
    if (FanDisconnectS[FAN_OFF_ON_DISCONNECT].s == ISS_ON)
        setFanEnabled(false);
 
    return INDI::Focuser::Disconnect();
}
 
bool EFA::Handshake()
{
    std::string version;
 
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 6;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x03;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FOC;
    cmd[4] = GET_VERSION;
    cmd[5] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    version = std::to_string(res[5]) + "." + std::to_string(res[6]);
    IUSaveText(&InfoT[INFO_VERSION], version.c_str());
 
    LOGF_INFO("Detected version %s", version.c_str());
 
    return true;
}
 
const char *EFA::getDefaultName()
{
    return "PlaneWave EFA";
}
 
bool EFA::ISNewSwitch(const char *dev, const char *name, ISState *states, char *names[], int n)
{
    if (dev != nullptr && strcmp(dev, getDeviceName()) == 0)
    {
        // Calibration State
        if (!strcmp(CalibrationStateSP.name, name))
        {
            bool enabled = strcmp(CalibrationStateS[CALIBRATION_ON].name, IUFindOnSwitchName(states, names, n)) == 0;
            if (setCalibrationEnabled(enabled))
            {
                IUUpdateSwitch(&CalibrationStateSP, states, names, n);
                CalibrationStateSP.s = IPS_OK;
            }
            else
            {
                CalibrationStateSP.s = IPS_ALERT;
            }
 
            IDSetSwitch(&CalibrationStateSP, nullptr);
            return true;
        }
        // Fan State
        else if (!strcmp(FanStateSP.name, name))
        {
            if (FanControlS[FAN_MANUAL].s == ISS_OFF)
            {
                FanStateSP.s = IPS_IDLE;
                LOG_WARN("Cannot control fan while manual control is turned off.");
                IDSetSwitch(&FanControlSP, nullptr);
                return true;
            }
 
            bool enabled = strcmp(FanStateS[FAN_ON].name, IUFindOnSwitchName(states, names, n)) == 0;
            if (setFanEnabled(enabled))
            {
                IUUpdateSwitch(&FanStateSP, states, names, n);
                FanStateSP.s = enabled ? IPS_OK : IPS_IDLE;
            }
            else
            {
                FanStateSP.s = IPS_ALERT;
            }
 
            IDSetSwitch(&FanStateSP, nullptr);
            return true;
        }
        // Fan Control
        else if (!strcmp(FanControlSP.name, name))
        {
            IUUpdateSwitch(&FanControlSP, states, names, n);
            if (FanControlS[FAN_MANUAL].s == ISS_ON)
            {
                deleteProperty(FanControlNP.name);
                LOG_INFO("Fan is now controlled manually.");
            }
            else
            {
                LOG_INFO("Fan is now controlled automatically per the control parameters.");
                defineProperty(&FanControlNP);
            }
 
            FanControlSP.s = IPS_OK;
            IDSetSwitch(&FanControlSP, nullptr);
            return true;
        }
        // Fan Disconnect
        else if (!strcmp(FanDisconnectSP.name, name))
        {
            IUUpdateSwitch(&FanDisconnectSP, states, names, n);
 
            if (FanDisconnectS[FAN_OFF_ON_DISCONNECT].s == ISS_ON)
                LOG_INFO("Fan shall be turned off upon device disconnection.");
            else
                LOG_INFO("Fan shall left as-is upon device disconnection.");
 
            FanDisconnectSP.s = IPS_OK;
            IDSetSwitch(&FanDisconnectSP, nullptr);
            return true;
        }
    }
 
    return INDI::Focuser::ISNewSwitch(dev, name, states, names, n);
}
 
bool EFA::ISNewNumber(const char *dev, const char *name, double values[], char *names[], int n)
{
    if (dev != nullptr && strcmp(dev, getDeviceName()) == 0)
    {
        // Fan Params
        if (!strcmp(FanControlNP.name, name))
        {
            IUUpdateNumber(&FanControlNP, values, names, n);
            FanControlNP.s = IPS_OK;
            IDSetNumber(&FanControlNP, nullptr);
            return true;
        }
    }
 
    return INDI::Focuser::ISNewNumber(dev, name, values, names, n);
}
 
bool EFA::SyncFocuser(uint32_t ticks)
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 9;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x06;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FOC;
    cmd[4] = MTR_OFFSET_CNT;
    cmd[5] = (ticks >> 16) & 0xFF;
    cmd[6] = (ticks >>  8) & 0xFF;
    cmd[7] = (ticks >>  0) & 0xFF;
    cmd[8] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    return (res[5] == 1);
}
 
IPState EFA::MoveAbsFocuser(uint32_t targetTicks)
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 9;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x06;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FOC;
    cmd[4] = MTR_GOTO_POS2;
    cmd[5] = (targetTicks >> 16) & 0xFF;
    cmd[6] = (targetTicks >>  8) & 0xFF;
    cmd[7] = (targetTicks >>  0) & 0xFF;
    cmd[8] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return IPS_ALERT;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return IPS_ALERT;
 
    return (res[5] == 1) ? IPS_BUSY : IPS_ALERT;
}
 
IPState EFA::MoveRelFocuser(FocusDirection dir, uint32_t ticks)
{
    int direction = (dir == FOCUS_INWARD) ? -1 : 1;
    int reversed = (FocusReverseS[INDI_ENABLED].s == ISS_ON) ? -1 : 1;
    int relative = static_cast<int>(ticks);
    int targetAbsPosition = FocusAbsPosN[0].value + (relative * direction * reversed);
 
    targetAbsPosition = std::min(static_cast<uint32_t>(FocusAbsPosN[0].max),
                                 static_cast<uint32_t>(std::max(static_cast<int>(FocusAbsPosN[0].min), targetAbsPosition)));
 
    return MoveAbsFocuser(targetAbsPosition);
}
 
void EFA::TimerHit()
{
    if (!isConnected())
        return;
 
    IN_TIMER = true;
 
    readPosition();
 
    if (readTemperature())
    {
        // Send temperature is above threshold
        bool aboveThreshold = false;
        for (int i = 0; i < TemperatureNP.nnp; i++)
        {
            if (std::fabs(TemperatureN[i].value - m_LastTemperature[i]) > TEMPERATURE_THRESHOLD)
            {
                aboveThreshold = true;
                m_LastTemperature[i] = TemperatureN[i].value;
            }
        }
 
        if (aboveThreshold)
            IDSetNumber(&TemperatureNP, nullptr);
 
 
        if (FanControlS[FAN_MANUAL].s == ISS_OFF)
        {
            bool turnOn = false, turnOff = false;
            const bool isFanOn = FanStateS[FAN_ON].s == ISS_ON;
 
            // Check if we need to do automatic regulation of fan
            if (FanControlS[FAN_AUTOMATIC_ABSOLUTE].s == ISS_ON)
            {
                // Adjust delta for deadzone
                double min_delta = FanControlN[FAN_MAX_ABSOLUTE].value - FanControlN[FAN_DEADZONE].value;
                double max_delta = FanControlN[FAN_MAX_ABSOLUTE].value + FanControlN[FAN_DEADZONE].value;
 
                turnOn = TemperatureN[TEMPERATURE_PRIMARY].value > max_delta;
                turnOff = TemperatureN[TEMPERATURE_PRIMARY].value < min_delta;
            }
            else if (FanControlS[FAN_AUTOMATIC_RELATIVE].s == ISS_ON)
            {
                // Temperature delta
                double tDiff = TemperatureN[TEMPERATURE_PRIMARY].value - TemperatureN[TEMPERATURE_AMBIENT].value;
                // Adjust delta for deadzone
                double min_delta = FanControlN[FAN_MAX_RELATIVE].value - FanControlN[FAN_DEADZONE].value;
                double max_delta = FanControlN[FAN_MAX_RELATIVE].value + FanControlN[FAN_DEADZONE].value;
 
                // Check if we need to turn off/on fan
                turnOn = tDiff > max_delta;
                turnOff = tDiff < min_delta;
            }
 
            if (isFanOn && turnOff)
            {
                setFanEnabled(false);
                FanStateS[FAN_ON].s = ISS_OFF;
                FanStateS[FAN_OFF].s = ISS_ON;
                FanStateSP.s = IPS_IDLE;
                IDSetSwitch(&FanStateSP, nullptr);
            }
            else if (!isFanOn && turnOn)
            {
                setFanEnabled(true);
                FanStateS[FAN_ON].s = ISS_ON;
                FanStateS[FAN_OFF].s = ISS_OFF;
                FanStateSP.s = IPS_OK;
                IDSetSwitch(&FanStateSP, nullptr);
            }
        }
    }
 
    if (FocusAbsPosNP.s == IPS_BUSY || FocusRelPosNP.s == IPS_BUSY)
    {
        if (isGOTOComplete())
        {
            FocusAbsPosNP.s = IPS_OK;
            FocusRelPosNP.s = IPS_OK;
            IDSetNumber(&FocusAbsPosNP, nullptr);
            IDSetNumber(&FocusRelPosNP, nullptr);
            LOG_INFO("Focuser reached requested position.");
        }
        else
            IDSetNumber(&FocusAbsPosNP, nullptr);
    }
    else if (std::fabs(FocusAbsPosN[0].value - m_LastPosition) > 0)
    {
        m_LastPosition = FocusAbsPosN[0].value;
        IDSetNumber(&FocusAbsPosNP, nullptr);
    }
 
    IN_TIMER = false;
 
    SetTimer(getCurrentPollingPeriod());
}
 
 
bool EFA::AbortFocuser()
{
    return false;
}
 
bool EFA::SetFocuserMaxPosition(uint32_t ticks)
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 9;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x06;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FOC;
    cmd[4] = MTR_SLEWLIMITMAX;
    cmd[5] = (ticks >> 16) & 0xFF;
    cmd[6] = (ticks >>  8) & 0xFF;
    cmd[7] = (ticks >>  0) & 0xFF;
    cmd[8] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    return (res[5] == 1);
}
 
bool EFA::ReverseFocuser(bool enabled)
{
    INDI_UNUSED(enabled);
    return true;
}
 
bool EFA::saveConfigItems(FILE * fp)
{
    INDI::Focuser::saveConfigItems(fp);
 
    IUSaveConfigSwitch(fp, &FanControlSP);
    if (FanControlNP.s == IPS_OK)
        IUSaveConfigNumber(fp, &FanControlNP);
    IUSaveConfigSwitch(fp, &FanDisconnectSP);
    return true;
}
 
void EFA::getStartupValues()
{
    readPosition();
    readCalibrationState();
    readFanState();
    readTemperature();
 
    if (readMaxSlewLimit())
    {
        FocusAbsPosN[0].max = FocusMaxPosN[0].max;
        FocusAbsPosN[0].step = FocusAbsPosN[0].max / 50;
 
        FocusRelPosN[0].value = FocusAbsPosN[0].max / 50;
        FocusRelPosN[0].max = FocusAbsPosN[0].max / 2;
        FocusRelPosN[0].step = FocusRelPosN[0].max / 50;
 
        FocusMaxPosN[0].value = FocusMaxPosN[0].max;
        FocusMaxPosN[0].step = FocusMaxPosN[0].max / 50;
 
        IUUpdateMinMax(&FocusRelPosNP);
        IUUpdateMinMax(&FocusAbsPosNP);
        IUUpdateMinMax(&FocusMaxPosNP);
    }
}
 
inline int EFA::readByte(int fd, uint8_t *buf, int timeout, int *nbytes_read)
{
    return tty_read(fd, (char *)buf, 1, timeout, nbytes_read);
}
 
inline int EFA::readBytes(int fd, uint8_t *buf, int nbytes, int timeout, int *nbytes_read)
{
    return tty_read(fd, (char *)buf, nbytes, timeout, nbytes_read);
}
 
inline int EFA::writeBytes(int fd, const uint8_t *buf, int nbytes, int *nbytes_written)
{
    return tty_write(fd, (const char *)buf, nbytes, nbytes_written);
}
 
int EFA::readPacket(int fd, uint8_t *buf, int nbytes, int timeout, int *nbytes_read)
{
    int len = 0, rc = 0, read_bytes = *nbytes_read = 0;
 
    if (nbytes < 6)   // smallest packet is 6 bytes
    {
        LOGF_ERROR("Read needs at least 6 bytes; exceeds supplied buffer size (%d)", nbytes);
        return TTY_READ_ERROR;
    }
 
    for (int i = 0; i < 10; i++)
    {
        // look for SOM byte
        rc = readByte(fd, &buf[0], timeout, &read_bytes);
        if (rc == TTY_OK && read_bytes == 1)
        {
            if (buf[0] == DRIVER_SOM)
            {
                break;
            }
            else
            {
                LOGF_DEBUG("Looking for SOM (%02X); found %d byte (%02X)", DRIVER_SOM, read_bytes, buf[0]);
            }
        }
        else
        {
            char errstr[MAXRBUF] = {0};
            tty_error_msg(rc, errstr, MAXRBUF);
            LOGF_DEBUG("Looking for SOM (%02X); found %s", DRIVER_SOM, errstr);
        }
    }
    if (rc != TTY_OK || read_bytes != 1 || buf[0] != DRIVER_SOM)
    {
        LOGF_DEBUG("%s byte not encountered", "SOM");
        if (rc == TTY_OK)
            return TTY_TIME_OUT;
        return rc;
    }
 
    if ((rc = readByte(fd, &buf[1], timeout, &read_bytes)) != TTY_OK)
    {
        LOGF_DEBUG("%s byte not encountered", "LEN");
        return rc;
    }
    len = buf[1];
 
    // read source
    if ((rc = readByte(fd, &buf[2], timeout, &read_bytes)) != TTY_OK)
    {
        LOGF_DEBUG("%s byte not encountered", "SRC");
        return rc;
    }
    // read receiver
    if ((rc = readByte(fd, &buf[3], timeout, &read_bytes)) != TTY_OK)
    {
        LOGF_DEBUG("%s byte not encountered", "RCV");
        return rc;
    }
    // read command
    if ((rc = readByte(fd, &buf[4], timeout, &read_bytes)) != TTY_OK)
    {
        LOGF_DEBUG("%s byte not encountered", "CMD");
        return rc;
    }
 
    if ((len + 3) > nbytes)
    {
        LOGF_ERROR("Read (%d) will exceed supplied buffer size (%d) for command %02X", (len + 3), nbytes, buf[4]);
        return TTY_READ_ERROR;
    }
 
    // read data
    int n;
    for (n = 0; n < (len - 3); ++n)
    {
        if ((rc = readByte(fd, &buf[5 + n], timeout, &read_bytes)) != TTY_OK)
        {
            LOGF_DEBUG("%s byte not encountered", "DATA");
            return rc;
        }
    }
    // read checksum
    if ((rc = readByte(fd, &buf[5 + n], timeout, &read_bytes)) != TTY_OK)
    {
        LOGF_DEBUG("%s byte not encountered", "DATA");
        return rc;
    }
 
    uint8_t chk = calculateCheckSum(buf, (len + 3));
 
    if (chk != buf[len + 2])
    {
        LOG_ERROR("Invalid checksum!");
        return TTY_OK; // not a tty error, nbytes_read is still zero and it is used to indicate there was this problem
    }
 
    *nbytes_read = len + 3;
 
    return rc;
}
 
bool EFA::sendCommand(const uint8_t * cmd, uint8_t *res, uint32_t cmd_len, uint32_t res_len)
{
    int nbytes_written = 0, nbytes_read = 0, bits = 0, rc = 0, hexbuflen = (DRIVER_LEN * 3 + 4);
    char hexbuf[DRIVER_LEN * 3 + 4];
 
    for (int j = 0; j < 3; usleep(100000), j++)
    {
        // make sure RTS is lowered
        bits = TIOCM_RTS;
        (void) ioctl(PortFD, TIOCMBIC, &bits);
        bits = 0;
 
        // Wait until CTS is cleared.
        for (int i = 0; i < 10; i++)
        {
            if ((rc = ioctl(PortFD, TIOCMGET, &bits)) == 0 && (bits & TIOCM_CTS) == 0)
                break;
            usleep(100000);
        }
 
        if (rc < 0 || (bits & TIOCM_CTS) != 0)
        {
            LOGF_ERROR("CTS timed out: %s", strerror(errno));
            return false;
        }
 
        // Now raise RTS
        bits = TIOCM_RTS;
        ioctl(PortFD, TIOCMBIS, &bits); // was TIOCMSET
 
        if (!IN_TIMER && efaDump(hexbuf, hexbuflen, cmd, cmd_len) != NULL)
        {
            LOGF_DEBUG("CMD: %s", hexbuf);
        }
        rc = writeBytes(PortFD, cmd, cmd_len, &nbytes_written);
 
        if (rc != TTY_OK)
        {
            continue;
        }
 
        rc = readPacket(PortFD, res, res_len, DRIVER_TIMEOUT, &nbytes_read);
 
        if (rc != TTY_OK || nbytes_read == 0)
        {
            continue;
        }
 
        if ((int)cmd_len == nbytes_read && memcmp(cmd, res, cmd_len) == 0)
        {
            // received an echo
 
            bits = TIOCM_RTS;
            ioctl(PortFD, TIOCMBIC, &bits);
 
            // Next read the actual response from EFA
            rc = readPacket(PortFD, res, res_len, DRIVER_TIMEOUT, &nbytes_read);
 
            if (rc != TTY_OK || nbytes_read == 0)
            {
                continue;
            }
        }
        else if (efaDump(hexbuf, hexbuflen, cmd, cmd_len) != NULL)
        {
            // expected there to always be an echo, so note this occurence
            LOGF_DEBUG("no echo for command packet: %s", hexbuf);
        }
 
        if (!IN_TIMER && efaDump(hexbuf, hexbuflen, res, nbytes_read) != NULL)
        {
            LOGF_DEBUG("RESP: %s", hexbuf);
        }
 
        if (cmd[2] != res[3] || cmd[3] != res[2] || cmd[4] != res[4])
        {
            LOGF_DEBUG("Send/Receive mismatch - %s!", "packet not for us");
            continue;
        }
        break;
    }
 
    // Extra lowering of RTS to make sure hand controller is made available
    bits = TIOCM_RTS;
    ioctl(PortFD, TIOCMBIC, &bits);
 
    if (rc != TTY_OK)
    {
        char errstr[MAXRBUF] = {0};
        tty_error_msg(rc, errstr, MAXRBUF);
        LOGF_ERROR("Serial I/O error: %s.", errstr);
        return false;
    }
 
    if (cmd[2] != res[3] || cmd[3] != res[2] || cmd[4] != res[4])
    {
        LOGF_ERROR("Send/Receive mismatch and %s", "timeout");
        return false;
    }
 
    return true;
}
 
bool EFA::readPosition()
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 6;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x03;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FOC;
    cmd[4] = MTR_GET_POS;
    cmd[5] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    FocusAbsPosN[0].value = res[5] << 16 | res[6] << 8 | res[7];
    return true;
}
 
bool EFA::readMaxSlewLimit()
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 6;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x03;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FOC;
    cmd[4] = MTR_SLEWLIMITGETMAX;
    cmd[5] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    uint32_t limit = res[5] << 16 | res[6] << 8 | res[7];
    if (limit > 0)
    {
        FocusMaxPosN[0].max = limit;
        return true;
    }
 
    return false;
 
}
 
bool EFA::isGOTOComplete()
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 6;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x03;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FOC;
    cmd[4] = MTR_GOTO_OVER;
    cmd[5] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    return (res[5] != 0);
}
 
bool EFA::setFanEnabled(bool enabled)
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 7;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x04;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FAN;
    cmd[4] = FANS_SET;
    cmd[5] = enabled ? 1 : 0;
    cmd[6] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    return (res[5] == 1);
}
 
bool EFA::readFanState()
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 6;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x03;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FAN;
    cmd[4] = FANS_GET;
    cmd[5] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    bool enabled = (res[5] == 0);
 
    FanStateS[FAN_ON].s  = enabled ? ISS_ON : ISS_OFF;
    FanStateS[FAN_OFF].s = enabled ? ISS_OFF : ISS_ON;
 
    return true;
}
 
bool EFA::setCalibrationEnabled(bool enabled)
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 8;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x05;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FOC;
    cmd[4] = MTR_SET_CALIBRATION_STATE;
    cmd[5] = 0x40;
    cmd[6] = enabled ? 1 : 0;
    cmd[7] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    return (res[5] == 1);
}
 
bool EFA::readCalibrationState()
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 7;
 
    cmd[0] = DRIVER_SOM;
    cmd[1] = 0x04;
    cmd[2] = DEVICE_PC;
    cmd[3] = DEVICE_FOC;
    cmd[4] = MTR_GET_CALIBRATION_STATE;
    cmd[5] = 0x40;
    cmd[6] = calculateCheckSum(cmd, len);
 
    if (!validateLengths(cmd, len))
        return false;
 
    if (!sendCommand(cmd, res, len, DRIVER_LEN))
        return false;
 
    bool enabled = (res[5] == 1);
 
    CalibrationStateS[CALIBRATION_ON].s  = enabled ? ISS_ON : ISS_OFF;
    CalibrationStateS[CALIBRATION_OFF].s = enabled ? ISS_OFF : ISS_ON;
 
    return true;
}
 
bool EFA::readTemperature()
{
    uint8_t cmd[DRIVER_LEN] = {0}, res[DRIVER_LEN] = {0}, len = 7;
 
    for (uint8_t i = 0; i < 2; i++)
    {
        cmd[0] = DRIVER_SOM;
        cmd[1] = 0x04;
        cmd[2] = DEVICE_PC;
        cmd[3] = DEVICE_TEMP;
        cmd[4] = TEMP_GET;
        cmd[5] = i;
        cmd[6] = calculateCheckSum(cmd, len);
 
        if (!validateLengths(cmd, len))
            return false;
 
        if (!sendCommand(cmd, res, len, DRIVER_LEN))
            return false;
 
        TemperatureN[i].value = calculateTemperature(res[5], res[6]);
    }
 
    return true;
}
 
double EFA::calculateTemperature(uint8_t byte2, uint8_t byte3)
{
    if (byte2 == 0x7F && byte3 == 0x7F)
        return -100;
 
    int raw_temperature = byte3 << 8 | byte2;
    if (raw_temperature & 0x8000)
        raw_temperature = raw_temperature - 0x10000;
 
    return raw_temperature / 16.0;
}
 
char * EFA::efaDump(char * buf, int buflen, const uint8_t * data, uint32_t size)
{
    int needed = 0, idx = 0;
 
    needed = size * 3 + 4; // each byte goes to 2 chars plus 1 space (or trailing null char) plus 4 marker characters
 
    if (needed > buflen)
    {
        return NULL;
    }
 
    for (uint32_t i = 0; i < size; i++)
    {
        if (i == 4)
        {
            (void) sprintf(buf + idx, "<%02X> ", data[i]);
            idx += 5;
        }
        else if (i == 5 && i < (size - 1))
        {
            buf[idx++] = '[';
            for (uint32_t j = i, k = 3; j < (size - 1) && k < data[1]; j++, k++)
            {
                (void) sprintf(buf + idx, "%02X ", data[j]);
                idx += 3;
                i = j;
            }
            buf[idx - 1] = ']';
            buf[idx++] = ' ';
        }
        else
        {
            (void) sprintf(buf + idx, "%02X ", data[i]);
            idx += 3;
        }
    }
 
    buf[idx - 1] = '\0';
 
    return buf;
}
 
std::vector<std::string> EFA::split(const std::string &input, const std::string &regex)
{
    // passing -1 as the submatch index parameter performs splitting
    std::regex re(regex);
    std::sregex_token_iterator
    first{input.begin(), input.end(), re, -1},
          last;
    return {first, last};
}
 
template <typename T>
std::string EFA::to_string(const T a_value, const int n)
{
    std::ostringstream out;
    out.precision(n);
    out << std::fixed << a_value;
    return out.str();
}
 
bool EFA::validateLengths(const uint8_t *cmd, uint32_t len)
{
    if (len < 6)
    {
        LOGF_ERROR("packet length (%d) is too short for command %02X", len, cmd[4]);
        return false;
    }
    if (cmd[1] + 3 != (int)len)
    {
        LOGF_ERROR("packet length (%d) and data length (%d) discrepancy for command %02X", len, cmd[1], cmd[4]);
        return false;
    }
 
    return true;
}
 
uint8_t EFA::calculateCheckSum(const uint8_t *cmd, uint32_t len)
{
    int32_t sum = 0;
 
    for (uint32_t i = 1; i < len - 1; i++)
        sum += cmd[i];
 
    return ((-sum) & 0xFF);
}