CSerialPort.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "comms-precomp.h"  // Precompiled headers

#include <mrpt/comms/CSerialPort.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/system/os.h>

#if defined(MRPT_OS_LINUX) || defined(__APPLE__)
// Linux implementation: refer to
//  http://www.easysw.com/~mike/serial/serial.html

#include <fcntl.h> /* File control definitions */
#include <sys/time.h>  // gettimeofday
#include <unistd.h> /* UNIX standard function definitions */
#include <cerrno> /* Error number definitions */
#include <cstdio> /* Standard input/output definitions */
#include <cstring> /* String function definitions */

#include <termios.h> /* POSIX terminal control definitions */

#include <sys/ioctl.h>  // FIONREAD,...
#include <csignal>

#ifdef HAVE_LINUX_SERIAL_H
#include <linux/serial.h>
#endif

#include <map>

#endif  // Linux | Apple

#ifdef _WIN32
#include <windows.h>
#endif

#include <iostream>
#include <thread>

using namespace mrpt;
using namespace mrpt::comms;
using namespace mrpt::io;
using namespace std;
using namespace std::literals;

// ctor
CSerialPort::CSerialPort(const string& portName, bool openNow)
    : m_serialName(portName)
{
    if (openNow) open();
}

// Dtor:
CSerialPort::~CSerialPort()
{
    if (isOpen()) close();
}

void CSerialPort::open(const std::string& COM_name)
{
    if (isOpen() && m_serialName != COM_name)
        THROW_EXCEPTION("Cannot change serial port while open");
    if (!isOpen())
    {
        setSerialPortName(COM_name);
        open();
    }
}

void CSerialPort::setSerialPortName(const std::string& COM_name)
{
    if (isOpen()) THROW_EXCEPTION("Cannot change serial port while open");
    m_serialName = COM_name;
}

/* -----------------------------------------------------
                Open
   ----------------------------------------------------- */
void CSerialPort::open()
{
    MRPT_START
#ifdef _WIN32
    // Check name:
    if (!m_serialName.size()) THROW_EXCEPTION("Serial port name is empty!!");

    // Is it COMX, X>4? ->  "\\.\COMX"
    if (tolower(m_serialName[0]) == 'c' && tolower(m_serialName[1]) == 'o' &&
        tolower(m_serialName[2]) == 'm')
    {
        // Need to add "\\.\"?
        if (m_serialName.size() > 4 || m_serialName[3] > '4')
            m_serialName = std::string("\\\\.\\") + m_serialName;
    }

    // Open the serial port:
    if (INVALID_HANDLE_VALUE ==
        (hCOM = CreateFileA(
             m_serialName.c_str(),  // Serial Port name
             GENERIC_READ | GENERIC_WRITE, 0, nullptr, OPEN_EXISTING, 0, 0)))
    {
        hCOM = nullptr;
        THROW_EXCEPTION_FMT(
            "Error trying to open serial port: %s", m_serialName.c_str());
    }

    // Set recommended buffer sizes:
    SetupComm(hCOM, 4096, 4096);

#else
    // Check name:
    if (!m_serialName.size()) THROW_EXCEPTION("Serial port name is empty!!");
    if (m_serialName[0] != '/') m_serialName = string("/dev/") + m_serialName;

    // Open the serial port:
    // The O_NOCTTY flag tells UNIX that this program doesn't want to be the
    // "controlling terminal" for that port.
    // The O_NDELAY flag tells UNIX that this program doesn't care what state
    // the DCD signal line is in - whether the other end of the port is up and
    // running.
    if (-1 ==
        (hCOM = ::open(m_serialName.c_str(), O_RDWR | O_NOCTTY | O_NDELAY)))
        THROW_EXCEPTION_FMT(
            "Error trying to open the serial port %s!!", m_serialName.c_str());

    // Clear flags:
    fcntl(hCOM, F_SETFL, 0);

    //
    // Start assembling the new port settings.
    //
    termios port_settings;
    bzero(&port_settings, sizeof(port_settings));

    //
    // Enable the receiver (CREAD) and ignore modem control lines
    // (CLOCAL).
    //
    port_settings.c_cflag |= CREAD | CLOCAL;

    //
    // Set the VMIN and VTIME parameters to zero by default. VMIN is
    // the minimum number of characters for non-canonical read and
    // VTIME is the timeout in deciseconds for non-canonical
    // read. Setting both of these parameters to zero implies that a
    // read will return immediately only giving the currently
    // available characters.
    //
    port_settings.c_cc[VMIN] = 0;
    port_settings.c_cc[VTIME] = 0;

    /*
     * Flush the input buffer associated with the port.
     */
    if (tcflush(hCOM, TCIFLUSH) < 0)
        THROW_EXCEPTION_FMT("Cannot flush serial port: %s", strerror(errno));

    /*
     * Write the new settings to the port.
     */
    if (tcsetattr(hCOM, TCSANOW, &port_settings) < 0)
        THROW_EXCEPTION_FMT(
            "Cannot set the new config to the serial port: %s",
            strerror(errno));

    // Do NOT block on read.
    fcntl(hCOM, F_SETFL, FNDELAY);

// Success!
#endif
    MRPT_END
}

/* -----------------------------------------------------
                isOpen
   ----------------------------------------------------- */
bool CSerialPort::isOpen() const
{
#ifdef _WIN32
    return hCOM != nullptr;
#else
    return hCOM != -1;
#endif
}

/* -----------------------------------------------------
                setConfig
   ----------------------------------------------------- */
void CSerialPort::setConfig(
    int baudRate, int parity, int bits, int nStopBits, bool enableFlowControl)
{
    MRPT_START
#ifdef _WIN32

    DCB dcb_conf;
    dcb_conf.DCBlength = sizeof(DCB);

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The serial port is not open");

    if (!GetCommState(hCOM, &dcb_conf))
        THROW_EXCEPTION("Error retrieving COM state");

    //
    // Apply baud rate
    //
    int BR;
    switch (baudRate)
    {
        case 300:
            BR = CBR_300;
            break;
        case 600:
            BR = CBR_600;
            break;
        case 1200:
            BR = CBR_1200;
            break;
        case 2400:
            BR = CBR_2400;
            break;
        case 4800:
            BR = CBR_4800;
            break;
        case 9600:
            BR = CBR_9600;
            break;
        case 19200:
            BR = CBR_19200;
            break;
        case 38400:
            BR = CBR_38400;
            break;
        case 57600:
            BR = CBR_57600;
            break;
        case 115200:
            BR = CBR_115200;
            break;
        default:
            BR = baudRate;
            // THROW_EXCEPTION_FMT("Invalid desired baud rate value:
            // %i",baudRate ) ;
            break;
    }

    dcb_conf.BaudRate = BR;

    dcb_conf.ByteSize = (BYTE)bits;
    dcb_conf.Parity = (BYTE)parity;

    // stop bits:
    switch (nStopBits)
    {
        case 1:
            dcb_conf.StopBits = ONESTOPBIT;
            break;
        case 2:
            dcb_conf.StopBits = TWOSTOPBITS;
            break;
        default:
            THROW_EXCEPTION_FMT("Invalid number of stop bits: %i", nStopBits);
            break;
    }

    dcb_conf.fBinary = true;
    dcb_conf.fParity = parity != 0;

    dcb_conf.fRtsControl =
        enableFlowControl ? RTS_CONTROL_HANDSHAKE : RTS_CONTROL_DISABLE;
    dcb_conf.fOutxCtsFlow = enableFlowControl;

    // Apply:
    if (!SetCommState(hCOM, &dcb_conf))
        THROW_EXCEPTION("Error changing COM state");

    // Assure:
    if (!GetCommState(hCOM, &dcb_conf))
        THROW_EXCEPTION("Error retrieving COM state");
    if (((int)dcb_conf.BaudRate) != baudRate)
        THROW_EXCEPTION("COM state verification after writing failed");

    m_baudRate = baudRate;

#else
    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The serial port is not open!");

    ASSERT_(baudRate > 0);

    //
    // Apply baud rate
    //
    int BR;
    bool special_rate = false;
    switch (baudRate)
    {
        case 50:
            BR = B50;
            break;
        case 75:
            BR = B75;
            break;
        case 110:
            BR = B110;
            break;
        case 134:
            BR = B134;
            break;
        case 200:
            BR = B200;
            break;
        case 300:
            BR = B300;
            break;
        case 600:
            BR = B600;
            break;
        case 1200:
            BR = B1200;
            break;
        case 2400:
            BR = B2400;
            break;
        case 4800:
            BR = B4800;
            break;
        case 9600:
            BR = B9600;
            break;
        case 19200:
            BR = B19200;
            break;
        case 38400:
            BR = B38400;
            break;
        case 57600:
            BR = B57600;
            break;
        case 115200:
            BR = B115200;
            break;
        case 230400:
            BR = B230400;
            break;
#ifdef B460800
        case 460800:
            BR = B460800;
            break;
#endif
#ifdef B500000
        case 500000:
            BR = B500000;
            break;
#endif
#ifdef B4000000
        case 576000:
            BR = B576000;
            break;
        case 921600:
            BR = B921600;
            break;
        case 1000000:
            BR = B1000000;
            break;
        case 1152000:
            BR = B1152000;
            break;
        case 1500000:
            BR = B1500000;
            break;
        case 2000000:
            BR = B2000000;
            break;
        case 2500000:
            BR = B2500000;
            break;
        case 3000000:
            BR = B3000000;
            break;
        case 3500000:
            BR = B3500000;
            break;
        case 4000000:
            BR = B4000000;
            break;
#endif
        default:
#ifdef HAVE_LINUX_SERIAL_H
            special_rate = true;
#else
            BR = baudRate;  // This is all we can try in that case...
#endif
            break;
    }

    if (special_rate)
    {
#ifdef HAVE_LINUX_SERIAL_H
        struct serial_struct serial;
        if (ioctl(hCOM, TIOCGSERIAL, &serial) < 0)
            THROW_EXCEPTION("error on TIOCGSERIAL ioctl");

        serial.custom_divisor = serial.baud_base / baudRate;
        if (!serial.custom_divisor) serial.custom_divisor = 1;
        const int actual_rate = serial.baud_base / serial.custom_divisor;

        serial.flags &= ~ASYNC_SPD_MASK;
        serial.flags |= ASYNC_SPD_CUST;  // We want to use our CUSTOM divisor.

        if (ioctl(hCOM, TIOCSSERIAL, &serial) < 0)
            THROW_EXCEPTION("error on TIOCSSERIAL ioctl");

        BR = B38400;  // It seems that 38400 means to the driver here to use our
        // custom divisor

        if (actual_rate != baudRate)
            cout << "[CSerialPort::setConfig] Setting custom baud rate to "
                 << actual_rate << ", the closer I can make to " << baudRate
                 << endl;
#else
        THROW_EXCEPTION("Custom serial port baud rates require linux/serial.h");
#endif
    }  // end specialRate
    else
    {
        // Normal baudrate: Just in case, undo possible custom divisors:
        //#ifdef HAVE_LINUX_SERIAL_H
        //      struct serial_struct serial;
        //      if (ioctl(hCOM, TIOCGSERIAL, &serial) < 0)
        //            THROW_EXCEPTION("error on TIOCGSERIAL ioctl");
        //
        //        serial.flags &= ~ASYNC_SPD_MASK;
        //
        //      if (ioctl(hCOM, TIOCSSERIAL, &serial) < 0)
        //            THROW_EXCEPTION("error on TIOCSSERIAL ioctl");
        //#endif
    }

    termios port_settings;
    if (tcgetattr(hCOM, &port_settings) < 0)
        THROW_EXCEPTION_FMT(
            "Cannot get the current settings: %s", strerror(errno));

    if ((cfsetispeed(&port_settings, BR) < 0) ||
        (cfsetospeed(&port_settings, BR) < 0))
        THROW_EXCEPTION_FMT(
            "Cannot change baudRate in setting structure: %s", strerror(errno));

    //
    // Set the character size.
    //
    port_settings.c_cflag &= ~CSIZE;
    switch (bits)
    {
        case 5:
            port_settings.c_cflag |= CS5;
            break;
        case 6:
            port_settings.c_cflag |= CS6;
            break;
        case 7:
            port_settings.c_cflag |= CS7;
            break;
        case 8:
            port_settings.c_cflag |= CS8;
            break;
        default:
            THROW_EXCEPTION_FMT("Invalid character size: %i", bits);
            break;
    }

    // parity  0:No parity, 1:Odd, 2:Even
    switch (parity)
    {
        case 2:
            port_settings.c_cflag |= PARENB;
            port_settings.c_cflag &= ~PARODD;
            port_settings.c_iflag |= INPCK;
            break;
        case 1:
            port_settings.c_cflag |= (PARENB | PARODD);
            port_settings.c_iflag |= INPCK;
            break;
        case 0:
            port_settings.c_cflag &= ~(PARENB);
            port_settings.c_iflag |= IGNPAR;
            break;
        default:
            THROW_EXCEPTION_FMT("Invalid parity selection: %i", parity);
            break;
    }

    // stop bits:
    switch (nStopBits)
    {
        case 1:
            port_settings.c_cflag &= ~(CSTOPB);
            break;
        case 2:
            port_settings.c_cflag |= CSTOPB;
            break;
        default:
            THROW_EXCEPTION_FMT("Invalid number of stop bits: %i", nStopBits);
            break;
    }

    //
    // Set the flow control.
    //
    if (enableFlowControl)
    {
        // RTS/CTS ON:
        port_settings.c_cflag |= CRTSCTS;
    }
    else
    {
        // none
        port_settings.c_cflag &= ~(CRTSCTS);
    }

    /* Write the new settings to the port.
     */
    if (tcsetattr(hCOM, TCSANOW, &port_settings) < 0)
        THROW_EXCEPTION_FMT("Cannot set the new settings: %s", strerror(errno));

    // Check:
    termios port_settings_verif;
    if (tcgetattr(hCOM, &port_settings_verif) < 0)
        THROW_EXCEPTION_FMT(
            "Cannot get the settings to verify: %s", strerror(errno));

#if 0
    if (!special_rate)
    {
        if (port_settings_verif.c_ispeed != port_settings.c_ispeed)
            THROW_EXCEPTION("Verification of changed baudrate(i) failed");
        if (port_settings_verif.c_ospeed != port_settings.c_ospeed)
            THROW_EXCEPTION("Verification of changed baudrate(i) failed");
    }

    if (port_settings_verif.c_cflag !=  port_settings.c_cflag)
        THROW_EXCEPTION("Verification of serial port flags failed");
#endif

    m_baudRate = baudRate;
#endif
    MRPT_END
}

void CSerialPort::setTimeouts(
    int ReadIntervalTimeout, int ReadTotalTimeoutMultiplier,
    int ReadTotalTimeoutConstant, int WriteTotalTimeoutMultiplier,
    int WriteTotalTimeoutConstant)
{
    MRPT_START
#ifdef _WIN32
    COMMTIMEOUTS timeouts;

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The COM port is not open");

    // Config:
    timeouts.ReadIntervalTimeout =
        ReadIntervalTimeout;  // Milisegundos entre dos bytes recibidos
    timeouts.ReadTotalTimeoutMultiplier =
        ReadTotalTimeoutMultiplier;  // Milisegundos de espera por cada byte a
    // recibir
    timeouts.ReadTotalTimeoutConstant =
        ReadTotalTimeoutConstant;  // Milisegundos de espera en cada operacion
    // de recepcion
    timeouts.WriteTotalTimeoutMultiplier =
        WriteTotalTimeoutMultiplier;  // Timeout de escritura no usado
    timeouts.WriteTotalTimeoutConstant =
        WriteTotalTimeoutConstant;  // Timeout de escritura no usado

    if (!SetCommTimeouts(hCOM, &timeouts))
        THROW_EXCEPTION("Error changing COM port timeout config");

// Success
#else
    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The serial port is not open!");

    // Save variables which are used in other methods:
    m_totalTimeout_ms = ReadTotalTimeoutConstant;
    m_interBytesTimeout_ms = ReadIntervalTimeout;

    // http://www.unixwiz.net/techtips/termios-vmin-vtime.html
    // VMIN & VTIME
    termios port_settings;
    if (tcgetattr(hCOM, &port_settings) < 0)
        THROW_EXCEPTION_FMT(
            "Cannot get the current settings: %s", strerror(errno));

    // We set VMIN=0 and VTIME=ReadIntervalTimeout (in thenth of seconds)
    //
    port_settings.c_cc[VMIN] = 0;
    port_settings.c_cc[VTIME] = max(1, ReadTotalTimeoutConstant / 100);

    /* Write the new settings to the port.
     */
    if (tcsetattr(hCOM, TCSANOW, &port_settings) < 0)
        THROW_EXCEPTION_FMT("Cannot set the new settings: %s", strerror(errno));
#endif
    MRPT_END
}

/* -----------------------------------------------------
                Close
   ----------------------------------------------------- */
void CSerialPort::close()
{
    MRPT_START
#ifdef _WIN32
    if (hCOM) CloseHandle(hCOM);
    hCOM = nullptr;
#else
    if (hCOM < 0) return;  // Already closed

    //    PosixSignalDispatcher& signal_dispatcher =
    //    PosixSignalDispatcher::Instance() ;
    //    signal_dispatcher.DetachHandler( SIGIO, *this ) ;
    // Close the serial port file descriptor.
    ::close(hCOM);

    hCOM = -1;  // Means the port is closed
#endif
    MRPT_END
}

/* -----------------------------------------------------
                read
   ----------------------------------------------------- */
size_t CSerialPort::Read(void* Buffer, size_t Count)
{
    MRPT_START
#ifdef _WIN32
    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

    DWORD actuallyRead;

    if (!ReadFile(
            hCOM,  // Handle,
            Buffer,  // Buffer
            (DWORD)Count,  // Max expected bytes
            &actuallyRead,  // Actually read bytes
            nullptr))
        THROW_EXCEPTION("Error reading from port!");

    return actuallyRead;
#else

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

    if (!Count) return 0;

    // Use the "m_totalTimeout_ms" global timeout
    //  and the "m_interBytesTimeout_ms" for inter-bytes:
    m_timer.Tic();

    size_t alreadyRead = 0;
    int leftTime = m_totalTimeout_ms - (int)(m_timer.Tac() * 1000);

    while (alreadyRead < Count && leftTime >= 0)
    {
        // Bytes waiting in the queue?
        // Check if we are still connected or there is an error...
        int waiting_bytes = 0;
        if (ioctl(hCOM, FIONREAD, &waiting_bytes) < 0)
        {
            if (errno == EIO)
            {
                // The port has been disconnect (for USB ports)
                this->close();
                return alreadyRead;
            }
        }

        // Are there any bytes??
        int nRead = 0;

        if (waiting_bytes > 0)
        {
            int nToRead = min((size_t)waiting_bytes, Count - alreadyRead);

            if ((nRead = ::read(hCOM, ((char*)Buffer) + alreadyRead, nToRead)) <
                0)
            {
                cerr << "[CSerialPort] read() returned " << nRead
                     << ", errno=" << errno << endl;
            }
            else
            {
                alreadyRead += nRead;
            }
        }
        else
        {
            // Nope...
        }

        if (alreadyRead < Count)
        {
            // Wait 1 more ms for new data to arrive.
            std::this_thread::sleep_for(1ms);
        }

        // Reset interbytes timer:
        leftTime = m_totalTimeout_ms - (int)(m_timer.Tac() * 1000);
        if (nRead > 0) leftTime = max(leftTime, m_interBytesTimeout_ms);
    }

    //    cout << "READ DONE: "<< alreadyRead << endl;
    return alreadyRead;
#endif
    MRPT_END
}

/** Reads one text line from the serial port in POSIX "canonical mode".
 *  This method reads from the serial port until one of the characters in \a
 * eol are found.
 */
std::string CSerialPort::ReadString(
    const int total_timeout_ms, bool* out_timeout, const char* eol_chars)
{
    MRPT_TRY_START
    // Calling ::ReadBuffer() many times would be even worse, so replicate its
    // code here:

    ASSERT_(eol_chars != nullptr);

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

    if (out_timeout) *out_timeout = false;  // Will be set to true on timeout

    m_timer.Tic();
    string receivedStr;  // Rx buffer

    while (total_timeout_ms < 0 || (m_timer.Tac() * 1e3 < total_timeout_ms))
    {
#ifdef _WIN32
        // Read just 1 byte:
        char buf[1];

        DWORD actuallyRead;
        if (!ReadFile(
                hCOM,  // Handle,
                buf,  // Buffer
                1,  // Max expected bytes
                &actuallyRead,  // Actually read bytes
                nullptr))
            THROW_EXCEPTION("Error reading from port!");

        if (actuallyRead)
        {  // Append to string, if it's not a control char:
            if (!strchr(eol_chars, buf[0]))
                receivedStr.push_back(buf[0]);
            else
            {  // end of string!
                return receivedStr;
            }
        }
        // If we are still here, string is not finished:
        std::this_thread::sleep_for(
            1ms);  // Wait 1 more ms for new data to arrive.
#else
        // Bytes waiting in the queue?
        // Check if we are still connected or there is an error...
        int waiting_bytes = 0;
        if (ioctl(hCOM, FIONREAD, &waiting_bytes) < 0)
        {
            if (errno == EIO)
            {  // The port has been disconnect (for USB ports)
                this->close();
                THROW_EXCEPTION("Error reading port before end of line");
            }
        }

        // Are there any bytes??
        int nRead = 0;
        if (waiting_bytes > 0)
        {
            // Read just 1 byte:
            char buf[1];
            if ((nRead = ::read(hCOM, buf, 1)) < 0)
            {
                cerr << "[CSerialPort] Error reading from port..." << endl;
            }
            if (nRead)
            {  // Append to string, if it's not a control char:
                if (!strchr(eol_chars, buf[0]))
                    receivedStr.push_back(buf[0]);
                else
                {  // end of string!
                    return receivedStr;
                }
            }
        }
        else
        {
            // we decide to move the sleep here to satisfy realtime requirement
            // in the case where we are waiting a n-length string at a frequency
            // greater than 1/n...
            std::this_thread::sleep_for(
                1ms);  // Wait 1 more ms for new data to arrive.
        }
// If we are still here, string is not finished:
#endif
    }

    // Timeout:
    if (out_timeout) *out_timeout = true;
    return receivedStr;
    MRPT_TRY_END
}

size_t CSerialPort::Write(const void* Buffer, size_t Count)
{
    MRPT_START
    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

#ifdef _WIN32
    DWORD actuallyWritten;
    if (!WriteFile(hCOM, Buffer, (DWORD)Count, &actuallyWritten, nullptr))
        THROW_EXCEPTION("Error writing to port!");
    return actuallyWritten;
#else
    // Write the data to the serial port. Keep retrying if EAGAIN
    // error is received.

    /** \todo Add support for write timeout here */
    struct timeval start, end;
    int num_of_bytes_written = -1;
    size_t total_bytes_written = 0;
    do
    {
        gettimeofday(&start, nullptr);
        num_of_bytes_written = write(
            hCOM, reinterpret_cast<const char*>(Buffer) + total_bytes_written,
            Count - total_bytes_written);
        // cout << "wr: " << num_of_bytes_written << " tot: " <<
        // total_bytes_written << " of " << Count << " err: " << errno << endl;
        if (num_of_bytes_written > 0)
            total_bytes_written += num_of_bytes_written;

        if (num_of_bytes_written < (int)Count)
        {
            // JL: These few lines are from the Player/Stage project:

            // need to do this sort of busy wait to ensure the right timing
            // although I've noticed you will get some anamolies that are
            // in the ms range; this could be a problem...
            int usecs;
            do
            {
                gettimeofday(&end, nullptr);
                usecs = (end.tv_sec - start.tv_sec) * 1000000 +
                        (end.tv_usec - start.tv_usec);
            } while (usecs < 60);
            // std::this_thread::sleep_for(1ms); // we'll continue writting is a
            // ms.
        }
    } while ((total_bytes_written < Count) && (!errno || EAGAIN == errno));
    //
    if (num_of_bytes_written <
        0)  // This means we exit the loop due to a bad "errno".
        THROW_EXCEPTION_FMT(
            "Error writing data to the serial port: %s", strerror(errno));

    // Make sure the queue is drained
    // Synchronous IO doesnt always work
    ::tcdrain(hCOM);

    // OK:
    return total_bytes_written;
#endif

    MRPT_END
}

void CSerialPort::purgeBuffers()
{
    MRPT_START

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

#ifdef _WIN32
    if (!PurgeComm(
            hCOM,
            PURGE_RXABORT | PURGE_RXCLEAR | PURGE_TXABORT | PURGE_TXCLEAR))
        THROW_EXCEPTION("Error during COM port purge");
#else
    /* Flush the input buffer associated with the port. */
    if (tcflush(hCOM, TCIFLUSH) < 0)
        THROW_EXCEPTION_FMT("Cannot flush serial port: %s", strerror(errno));
#endif

    MRPT_END
}

uint64_t CSerialPort::Seek(int64_t Offset, CStream::TSeekOrigin Origin)
{
    MRPT_START
    MRPT_UNUSED_PARAM(Origin);
    MRPT_UNUSED_PARAM(Offset);
    THROW_EXCEPTION(
        "Method not applicable to serial communications port CStream!");
    MRPT_END
}

uint64_t CSerialPort::getTotalBytesCount() const
{
    MRPT_START
    THROW_EXCEPTION(
        "Method not applicable to serial communications port CStream!");
    MRPT_END
}

uint64_t CSerialPort::getPosition() const
{
    MRPT_START
    THROW_EXCEPTION(
        "Method not applicable to serial communications port CStream!");
    MRPT_END
}