CGPSInterface.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, 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 "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/comms/CClientTCPSocket.h>
#include <mrpt/hwdrivers/CGPSInterface.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <iostream>
#include <list>
#include <mutex>
#include <thread>

using namespace mrpt::hwdrivers;
using namespace mrpt::obs;
using namespace mrpt::system;
using namespace mrpt::comms;
using namespace std;

IMPLEMENTS_GENERIC_SENSOR(CGPSInterface, mrpt::hwdrivers)

struct TParsersRegistry
{
    std::list<CGPSInterface::ptr_parser_t> all_parsers;

    static const TParsersRegistry& getInstance()
    {
        static TParsersRegistry reg;
        return reg;
    }

   private:
    TParsersRegistry()
    {
        all_parsers.push_back(&CGPSInterface::implement_parser_NMEA);
        all_parsers.push_back(&CGPSInterface::implement_parser_NOVATEL_OEM6);
    }
};

/* -----------------------------------------------------
                Constructor
   ----------------------------------------------------- */
CGPSInterface::CGPSInterface()
    : mrpt::system::COutputLogger("CGPSInterface"),
      m_customInit(),
      m_rx_buffer(0x10000),
      m_raw_dump_file_prefix(),
      m_COMname(),
      m_last_timestamp(INVALID_TIMESTAMP),
      m_JAVAD_rtk_src_port(),
      m_JAVAD_rtk_format("cmr")
{
    m_sensorLabel = "GPS";
}

/* -----------------------------------------------------
                loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CGPSInterface::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource,
    const std::string& iniSection)
{
    m_parser = configSource.read_enum<CGPSInterface::PARSERS>(
        iniSection, "parser", m_parser, false /*Allow default values*/);
    m_raw_dump_file_prefix = configSource.read_string(
        iniSection, "raw_dump_file_prefix", m_raw_dump_file_prefix,
        false /*Allow default values*/);

#ifdef _WIN32
    m_COMname =
        configSource.read_string(iniSection, "COM_port_WIN", m_COMname, true);
#else
    m_COMname =
        configSource.read_string(iniSection, "COM_port_LIN", m_COMname, true);
#endif

    m_COMbauds =
        configSource.read_int(iniSection, "baudRate", m_COMbauds, true);
    m_sensorLabelAppendMsgType = configSource.read_bool(
        iniSection, "sensor_label_append_msg_type", m_sensorLabelAppendMsgType);

    // legacy custom cmds:
    m_customInit =
        configSource.read_string(iniSection, "customInit", m_customInit, false);

    // new custom cmds:
    m_custom_cmds_delay = configSource.read_float(
        iniSection, "custom_cmds_delay", m_custom_cmds_delay);
    m_custom_cmds_append_CRLF = configSource.read_bool(
        iniSection, "custom_cmds_append_CRLF", m_custom_cmds_append_CRLF);
    // Load as many strings as found on the way:
    m_setup_cmds.clear();
    for (int i = 1; true; i++)
    {
        std::string sLine = configSource.read_string(
            iniSection, mrpt::format("setup_cmd%i", i), std::string());
        sLine = mrpt::system::trim(sLine);
        if (sLine.empty()) break;
        m_setup_cmds.push_back(sLine);
    }

    m_shutdown_cmds.clear();
    for (int i = 1; true; i++)
    {
        std::string sLine = configSource.read_string(
            iniSection, mrpt::format("shutdown_cmd%i", i), std::string());
        sLine = mrpt::system::trim(sLine);
        if (sLine.empty()) break;
        m_shutdown_cmds.push_back(sLine);
    }

    m_sensorPose.setFromValues(
        configSource.read_float(iniSection, "pose_x", 0),
        configSource.read_float(iniSection, "pose_y", 0),
        configSource.read_float(iniSection, "pose_z", 0),
        DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
        DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
        DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));

    m_JAVAD_rtk_src_port = configSource.read_string(
        iniSection, "JAVAD_rtk_src_port", m_JAVAD_rtk_src_port);
    m_JAVAD_rtk_src_baud = configSource.read_int(
        iniSection, "JAVAD_rtk_src_baud", m_JAVAD_rtk_src_baud);
    m_JAVAD_rtk_format = configSource.read_string(
        iniSection, "JAVAD_rtk_format", m_JAVAD_rtk_format);

    m_topcon_useAIMMode = configSource.read_bool(
        iniSection, "JAVAD_useAIMMode", m_topcon_useAIMMode);
    m_topcon_data_period =
        1.0 / configSource.read_double(
                  iniSection, "outputRate", m_topcon_data_period);
}

CGPSInterface::~CGPSInterface() { OnConnectionShutdown(); }

void CGPSInterface::setParser(CGPSInterface::PARSERS parser)
{
    m_parser = parser;
}
CGPSInterface::PARSERS CGPSInterface::getParser() const { return m_parser; }
void CGPSInterface::bindStream(
    const std::shared_ptr<mrpt::io::CStream>& external_stream,
    const std::shared_ptr<std::mutex>& csOptionalExternalStream)
{
    m_data_stream_is_external = true;
    m_data_stream = external_stream;
    m_data_stream_cs = csOptionalExternalStream ? csOptionalExternalStream
                                                : m_data_stream_mine_cs;
}
void CGPSInterface::setSetupCommandsDelay(const double delay_secs)
{
    m_custom_cmds_delay = delay_secs;
}
double CGPSInterface::getSetupCommandsDelay() const
{
    return m_custom_cmds_delay;
}
void CGPSInterface::setSetupCommands(const std::vector<std::string>& cmds)
{
    m_setup_cmds = cmds;
}
const std::vector<std::string>& CGPSInterface::getSetupCommands() const
{
    return m_setup_cmds;
}
void CGPSInterface::setShutdownCommands(const std::vector<std::string>& cmds)
{
    m_shutdown_cmds = cmds;
}
const std::vector<std::string>& CGPSInterface::getShutdownCommands() const
{
    return m_shutdown_cmds;
}
void CGPSInterface::enableSetupCommandsAppendCRLF(const bool enable)
{
    m_custom_cmds_append_CRLF = enable;
}
bool CGPSInterface::isEnabledSetupCommandsAppendCRLF() const
{
    return m_custom_cmds_append_CRLF;
}

/* -----------------------------------------------------
                setSerialPortName
----------------------------------------------------- */
void CGPSInterface::setSerialPortName(const std::string& COM_port)
{
    // Dont allow changing the serial port if:
    if (m_data_stream_is_external)
        THROW_EXCEPTION(
            "Cannot change serial port name: an external stream has been "
            "already bound manually.");

    if (m_data_stream)
    {
        std::lock_guard<std::mutex> lock(*m_data_stream_cs);
        auto serial =
            dynamic_cast<mrpt::comms::CSerialPort*>(m_data_stream.get());
        if (serial && serial->isOpen())
            THROW_EXCEPTION(
                "Cannot change serial port name when it is already open");
    }

    // OK:
    m_COMname = COM_port;
}

/* -----------------------------------------------------
                getSerialPortName
----------------------------------------------------- */
std::string CGPSInterface::getSerialPortName() const { return m_COMname; }
/* -----------------------------------------------------
                tryToOpenTheCOM
----------------------------------------------------- */
bool CGPSInterface::tryToOpenTheCOM()
{
    // If this is the first use of the COM port, create it:
    if (!m_data_stream)
    {
        m_data_stream = std::make_shared<mrpt::comms::CSerialPort>();
        m_data_stream_is_external = false;
        m_data_stream_cs = m_data_stream_mine_cs;
    }

    auto serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
    if (serial)
    {
        {
            std::lock_guard<std::mutex> lock(*m_data_stream_cs);
            if (serial->isOpen()) return true;  // Already open

            if (m_verbose)
                cout << "[CGPSInterface] Opening " << m_COMname << " @ "
                     << m_COMbauds << endl;
        }
        try
        {
            serial->open(m_COMname);
            // Config:
            serial->setConfig(m_COMbauds, 0, 8, 1);
            serial->setTimeouts(1, 0, 1, 1, 1);

            // Do extra initialization?
            if (!OnConnectionEstablished())
            {
                serial->close();
                return false;
            }
            return true;  // All OK
        }
        catch (const std::exception& e)
        {
            std::cerr << "[CGPSInterface::tryToOpenTheCOM] Error opening or "
                         "configuring serial port:"
                      << std::endl
                      << e.what();
            serial->close();
            return false;
        }
    }  // end of this is a serial port

    return true;  // All OK
}

/* -----------------------------------------------------
                isGPS_connected
----------------------------------------------------- */
bool CGPSInterface::isGPS_connected() { return m_GPS_comsWork; }
/* -----------------------------------------------------
                doProcess
----------------------------------------------------- */
void CGPSInterface::doProcess()
{
    // Is the COM open?
    if (!tryToOpenTheCOM())
    {
        m_state = ssError;
        THROW_EXCEPTION("Could not open the input stream");
    }
    ASSERT_(m_data_stream != nullptr);
    auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
    auto* stream_tcpip = dynamic_cast<CClientTCPSocket*>(m_data_stream.get());

    // Read as many bytes as available:
    uint8_t buf[0x1000];
    const size_t to_read =
        std::min(m_rx_buffer.available() - 1, sizeof(buf) - 1);
    try
    {
        size_t nRead = 0;
        if (to_read > 0)
        {
            std::lock_guard<std::mutex> lock(*m_data_stream_cs);
            if (stream_tcpip)
            {
                nRead = stream_tcpip->readAsync(buf, to_read, 100, 10);
            }
            else if (stream_serial)
            {
                nRead = stream_serial->Read(buf, to_read);
            }
            else
            {
                nRead = m_data_stream->Read(buf, to_read);
            }
        }

        if (nRead) m_rx_buffer.push_many(buf, nRead);

        // Also dump to raw file:
        if (!m_raw_dump_file_prefix.empty() &&
            !m_raw_output_file.fileOpenCorrectly())
        {
            // 1st time open:
            mrpt::system::TTimeParts parts;
            mrpt::system::timestampToParts(now(), parts, true);
            string sFilePostfix = "_";
            sFilePostfix += format(
                "%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year,
                (unsigned int)parts.month, (unsigned int)parts.day,
                (unsigned int)parts.hour, (unsigned int)parts.minute,
                (unsigned int)parts.second);
            const string sFileName =
                m_raw_dump_file_prefix +
                mrpt::system::fileNameStripInvalidChars(sFilePostfix) +
                string(".gps");

            if (m_verbose)
                std::cout << "[CGPSInterface] Creating RAW dump file: `"
                          << sFileName << "`\n";
            m_raw_output_file.open(sFileName);
        }
        if (nRead && m_raw_output_file.fileOpenCorrectly())
        {
            m_raw_output_file.Write(buf, nRead);
        }
    }
    catch (std::exception&)
    {
        // ERROR:
        MRPT_LOG_ERROR(
            "[CGPSInterface::doProcess] Error reading stream of data: Closing "
            "communications\n");
        if (stream_serial)
        {
            std::lock_guard<std::mutex> lock(*m_data_stream_cs);
            stream_serial->close();
        }
        m_GPS_comsWork = false;
        return;
    }

    // Try to parse incomming data as messages:
    parseBuffer();

    // Decide whether to push out a new observation in old legacy mode.
    if (!m_customInit.empty())
    {  // "Advanced" (RTK,mmGPS) device  (kept for backwards-compatibility)
        bool do_append_obs = false;
        // FAMD
        // Append observation if:
        // 0. the timestamp seems to be correct!
        // 1. it contains both synched GGA and RMC data
        // 2. it contains only GGA or RMC but the next one is not synched with
        // it
        if (m_last_timestamp == INVALID_TIMESTAMP)
        {
            if (m_verbose)
                cout << "[CGPSInterface] Initial timestamp: "
                     << mrpt::system::timeToString(
                            m_just_parsed_messages.timestamp)
                     << endl;
            // Check if the initial timestamp seems to be OK (not a spurio one)
            TTimeStamp tmNow = mrpt::system::now();
            const double tdif = mrpt::system::timeDifference(
                m_just_parsed_messages.timestamp, tmNow);
            if (tdif >= 0 && tdif < 7500 /*Up to two hours*/)
                m_last_timestamp = m_just_parsed_messages.timestamp;
            else
            {
                if (m_verbose)
                    cout << "[CGPSInterface] Warning: The initial timestamp "
                            "seems to be wrong! : "
                         << tdif << endl;
            }
        }  // end-if
        else
        {
            const double time_diff = mrpt::system::timeDifference(
                m_last_timestamp, m_just_parsed_messages.timestamp);
            if (time_diff < 0 || time_diff > 300)  // Assert that the current
            // timestamp is after the
            // previous one and not more
            // than 5 minutes later ->
            // remove spurious
            {
                if (m_verbose)
                    cout << "[CGPSInterface ] Bad timestamp difference" << endl;
                return;
            }

            if (time_diff - m_topcon_data_period > 0.25 * m_topcon_data_period)
            {
                if (m_verbose)
                    cout << "[CGPSInterface] WARNING: According to the "
                            "timestamps, we probably skipped one frame!"
                         << endl;
            }

            // a. These GPS data have both synched RMC and GGA data
            // don't append observation until we have both data
            do_append_obs =
                (m_just_parsed_messages.has_GGA_datum() &&
                 m_just_parsed_messages.has_RMC_datum());
        }  // end-else

        if (do_append_obs) flushParsedMessagesNow();
    }
}

/** Queue out now the messages in \a m_just_parsed_messages, leaving it empty */

void CGPSInterface::flushParsedMessagesNow()
{
    // Generic observation data:
    m_just_parsed_messages.sensorPose = m_sensorPose;
    if (m_sensorLabelAppendMsgType)
        m_just_parsed_messages.sensorLabel =
            m_sensorLabel + string("_") + m_just_parsed_messages.sensorLabel;
    else
        m_just_parsed_messages.sensorLabel = m_sensorLabel;
    // Add observation to the output queue:
    CObservationGPS::Ptr newObs = std::make_shared<CObservationGPS>();
    m_just_parsed_messages.swap(*newObs);
    CGenericSensor::appendObservation(newObs);
    m_just_parsed_messages.clear();
    m_last_timestamp = m_just_parsed_messages.timestamp;

    // And this means the comms works:
    m_GPS_comsWork = true;
    m_state = ssWorking;
}

/* -----------------------------------------------------
                    parseBuffer
----------------------------------------------------- */
void CGPSInterface::parseBuffer()
{
    if (m_parser == CGPSInterface::NONE) return;  // Dont try to parse data

    // Only one parser selected?
    ptr_parser_t parser_ptr = nullptr;
    switch (m_parser)
    {
        case CGPSInterface::NMEA:
            parser_ptr = &CGPSInterface::implement_parser_NMEA;
            break;
        case CGPSInterface::NOVATEL_OEM6:
            parser_ptr = &CGPSInterface::implement_parser_NOVATEL_OEM6;
            break;
        case CGPSInterface::AUTO:
            break;  // Leave it as NULL
        default:
            throw std::runtime_error("[CGPSInterface] Unknown parser!");
    };
    if (parser_ptr)
    {
        // Use only one parser ----------
        size_t min_bytes;
        do
        {
            if (!(*this.*parser_ptr)(min_bytes))
            {
                if (m_rx_buffer.size() != 0)
                    m_rx_buffer.pop();  // Not the start of a frame, skip 1 byte
            }
            if (m_customInit.empty() /* If we are not in old legacy mode */ &&
                !m_just_parsed_messages.messages.empty())
                flushParsedMessagesNow();
        } while (m_rx_buffer.size() >= min_bytes);
    }  // end one parser mode ----------
    else
    {
        // AUTO mode --------
        const std::list<CGPSInterface::ptr_parser_t>& all_parsers =
            TParsersRegistry::getInstance().all_parsers;

        size_t global_min_bytes_max = 0;
        do
        {
            bool all_parsers_want_to_skip = true;
            for (auto all_parser : all_parsers)
            {
                parser_ptr = all_parser;
                size_t this_parser_min_bytes;
                if ((*this.*parser_ptr)(this_parser_min_bytes))
                    all_parsers_want_to_skip = false;
                mrpt::keep_max(global_min_bytes_max, this_parser_min_bytes);
            }

            if (all_parsers_want_to_skip && m_rx_buffer.size() != 0)
                m_rx_buffer.pop();  // Not the start of a frame, skip 1 byte

            if (m_customInit.empty() /* If we are not in old legacy mode */ &&
                !m_just_parsed_messages.messages.empty())
                flushParsedMessagesNow();
        } while (m_rx_buffer.size() >= global_min_bytes_max);
    }  // end AUTO mode ----
}

/* -----------------------------------------------------
                    JAVAD_sendMessage
----------------------------------------------------- */
void CGPSInterface::JAVAD_sendMessage(const char* str, bool waitForAnswer)
{
    if (!str) return;
    const size_t len = strlen(str);
    auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
    if (!stream_serial) return;

    size_t written;

    {
        std::lock_guard<std::mutex> lock(*m_data_stream_cs);
        written = stream_serial->Write(str, len);
    }

    if (m_verbose) std::cout << "[CGPSInterface] TX: " << str;

    if (written != len)
        throw std::runtime_error(
            format("Error sending command: '%s'", str).c_str());
    std::this_thread::sleep_for(5ms);

    if (!waitForAnswer) return;

    std::this_thread::sleep_for(200ms);
    char buf[200];
    buf[0] = '\0';

    int bad_counter = 0;
    while (bad_counter < 10)
    {
        size_t nRead;
        {
            std::lock_guard<std::mutex> lock(*m_data_stream_cs);
            written = stream_serial->Write(str, len);
            nRead = stream_serial->Read(buf, sizeof(buf));
        }

        if (m_verbose) std::cout << "[CGPSInterface] RX: " << buf << std::endl;

        if (nRead < 3)
            throw std::runtime_error(format(
                "ERROR: Invalid response '%s' for command '%s'", buf, str));

        if (nRead >= 3 && buf[0] == 'R' && buf[1] == 'E')
            return;  // Ok!
        else
            ++bad_counter;
    }
    throw std::runtime_error(
        format("ERROR: Invalid response '%s' for command '%s'", buf, str));
}

bool CGPSInterface::OnConnectionShutdown()
{
    auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());

    if (stream_serial && !stream_serial->isOpen()) return false;

    // Send commands:
    for (const auto& m_shutdown_cmd : m_shutdown_cmds)
    {
        if (m_verbose)
            cout << "[CGPSInterface] TX shutdown command: `" << m_shutdown_cmd
                 << "`\n";

        std::string sTx = m_shutdown_cmd;
        if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");
        try
        {
            std::lock_guard<std::mutex> lock(*m_data_stream_cs);
            m_data_stream->Write(&sTx[0], sTx.size());
        }
        catch (...)
        {
            return false;  // On any I/O error
        }

        std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
            m_custom_cmds_delay * 1000));
    }
    return true;
}

/* -----------------------------------------------------
                    OnConnectionEstablished
----------------------------------------------------- */
bool CGPSInterface::OnConnectionEstablished()
{
    m_last_GGA.clear();  // On comms reset, empty this cache
    m_just_parsed_messages.clear();

    // Legacy behavior:
    if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
        !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
    {
        return legacy_topcon_setup_commands();
    }

    // Purge input:
    auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
    if (stream_serial)
    {
        std::lock_guard<std::mutex> lock(*m_data_stream_cs);
        stream_serial->purgeBuffers();
    }

    // New behavior: Send generic commands set-up by the user in the config
    // file.

    // Send commands:
    for (const auto& m_setup_cmd : m_setup_cmds)
    {
        if (m_verbose)
            cout << "[CGPSInterface] TX setup command: `" << m_setup_cmd
                 << "`\n";

        std::string sTx = m_setup_cmd;
        if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");

        try
        {
            std::lock_guard<std::mutex> lock(*m_data_stream_cs);
            m_data_stream->Write(&sTx[0], sTx.size());
        }
        catch (const std::exception& e)
        {
            std::cerr << "[CGPSInterface::OnConnectionEstablished] Error "
                         "sending setup cmds: "
                      << e.what() << std::endl;
            return false;
        }
        std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
            m_custom_cmds_delay * 1000));
    }
    std::this_thread::sleep_for(
        std::chrono::duration<double, std::milli>(m_custom_cmds_delay * 1000));
    return true;
}

bool CGPSInterface::unsetJAVAD_AIM_mode()
{
    MRPT_START
    if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
        !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
    {
        // Stop messaging:
        JAVAD_sendMessage("%%dm\r\n", false);
        std::this_thread::sleep_for(500ms);
        JAVAD_sendMessage("%%dm\r\n", false);
        std::this_thread::sleep_for(1000ms);

        // Purge input:
        auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
        if (stream_serial)
        {
            std::lock_guard<std::mutex> lock(*m_data_stream_cs);
            stream_serial->purgeBuffers();
        }

        JAVAD_sendMessage("%%set,/par/cur/term/imode,cmd\r\n");  // set the
        // current port
        // in command
        // mode
        return true;
    }
    else
        return true;
    MRPT_END
}  // end-unsetJAVAD_AIM_mode

bool CGPSInterface::setJAVAD_AIM_mode()
{
    MRPT_START
    if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") ||
        !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
    {
        JAVAD_sendMessage(
            format("%%%%set,/par%s/imode,cmd\r\n", m_JAVAD_rtk_src_port.c_str())
                .c_str());  // set the port in command mode
        JAVAD_sendMessage(
            "%%set,/par/cur/term/jps/0,{nscmd,37,n,\"\"}\r\n");  // any command
        // starting
        // with % will
        // be treated
        // as normal

        ASSERT_(!m_JAVAD_rtk_format.empty());
        cout << "Formato de correcciones para GR3: " << m_JAVAD_rtk_format
             << endl;
        if (m_JAVAD_rtk_format == "cmr")
        {
            JAVAD_sendMessage(
                format(
                    "%%%%set,/par/cur/term/jps/1,{cmr,-1,y,%s}\r\n",
                    m_JAVAD_rtk_src_port.c_str())
                    .c_str());  // set corrections type CMR or CMR+
            JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
            JAVAD_sendMessage(format(
                                  "%%%%set,/par%s/imode,cmr\r\n",
                                  m_JAVAD_rtk_src_port.c_str())
                                  .c_str());
        }
        else if (m_JAVAD_rtk_format == "rtcm")
        {
            JAVAD_sendMessage(
                format(
                    "%%%%set,/par/cur/term/jps/1,{rtcm,-1,y,%s}\r\n",
                    m_JAVAD_rtk_src_port.c_str())
                    .c_str());  // set corrections type RTCM
            JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
            JAVAD_sendMessage(format(
                                  "%%%%set,/par%s/imode,rtcm\r\n",
                                  m_JAVAD_rtk_src_port.c_str())
                                  .c_str());
        }
        else if (m_JAVAD_rtk_format == "rtcm3")
        {
            JAVAD_sendMessage(
                format(
                    "%%%%set,/par/cur/term/jps/1,{rtcm3,-1,y,%s}\r\n",
                    m_JAVAD_rtk_src_port.c_str())
                    .c_str());  // set corrections type RTCM 3.x
            JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
            JAVAD_sendMessage(format(
                                  "%%%%set,/par%s/imode,rtcm3\r\n",
                                  m_JAVAD_rtk_src_port.c_str())
                                  .c_str());
        }
        else
        {
            cout << "Unknown RTK corrections format. Only supported: CMR, RTCM "
                    "or RTCM3"
                 << endl;
            return false;
        }
        JAVAD_sendMessage("%%set,/par/cur/term/imode,jps\r\n");  // sets current
        // port into
        // "JPS" mode

        return true;

    }  // end-if
    else
        return true;
    MRPT_END
}  // end-setJAVAD_AIM_mode

std::string CGPSInterface::getLastGGA(bool reset)
{
    std::string ret = m_last_GGA;
    if (reset) m_last_GGA.clear();
    return ret;
}

bool CGPSInterface::legacy_topcon_setup_commands()
{
    // Stop messaging:
    JAVAD_sendMessage("%%dm\r\n", false);
    std::this_thread::sleep_for(500ms);
    JAVAD_sendMessage("%%dm\r\n", false);
    std::this_thread::sleep_for(1000ms);

    // Purge input:
    auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
    if (stream_serial)
    {
        std::lock_guard<std::mutex> lock(*m_data_stream_cs);
        stream_serial->purgeBuffers();
    }

    // Configure RTK mode and source:
    if (m_verbose) cout << "[CGPSInterface] Configure RTK options" << endl;

    if (!m_JAVAD_rtk_src_port.empty())
    {
        const int elevation_mask = 5;  // Degs

        JAVAD_sendMessage(
            format("%%%%set,/par/lock/elm,%i\r\n", elevation_mask)
                .c_str());  // Set elevation mask to track satellites
        JAVAD_sendMessage(
            "%%set,/par/base/mode/,off\r\n");  // Set Base Mode off
        JAVAD_sendMessage("%%set,/par/pos/pd/period,1.0\r\n");  // Differential
        // Correction
        // Interval
        // JAVAD_sendMessage("%%set,hd/mode,off\r\n");  // fixed distance to rtk
        // base: Off
        // JAVAD_sendMessage("%%set,/par/pos/pd/hd/mode,off\r\n");  // fixed
        // distance to rtk base: Off  <-- Not working with TopCon GR3! (option
        // disabled)
        JAVAD_sendMessage(
            "%%set,/par/pos/pd/qcheck,off\r\n");  // Set Quality Checks Off
        JAVAD_sendMessage(
            "%%set,/par/pos/mode/cur,pd\r\n");  // Pos Mode Phase Diff
        JAVAD_sendMessage(
            "%%set,/par/pos/pd/textr,10\r\n");  // RTK Extrapolation Limit
        JAVAD_sendMessage(
            "%%set,/par/pos/pd/inuse,-1\r\n");  // Set Rovers Reference Station
        JAVAD_sendMessage("%%set,/par/pos/pd/nrs/mode,y\r\n");  // Enable
        // Nearest
        // Reference
        // Station Mode
        JAVAD_sendMessage("%%set,/par/pos/pd/mode,extrap\r\n");  // Enable
        // EXTRAPOLATED
        // mode in RTK
        // corrections

        // Set Differential Correction Source
        JAVAD_sendMessage(
            format(
                "%%%%set,/par/pos/pd/port,%s\r\n", m_JAVAD_rtk_src_port.c_str())
                .c_str());

        // Set port bauds:
        if (!m_topcon_useAIMMode && m_JAVAD_rtk_src_baud != 0 &&
            !mrpt::system::strCmp(m_JAVAD_rtk_src_port, "/dev/usb/a"))
            JAVAD_sendMessage(format(
                                  "%%%%set,/par%s/rate,%u\r\n",
                                  m_JAVAD_rtk_src_port.c_str(),
                                  m_JAVAD_rtk_src_baud)
                                  .c_str());

        // Set Input Mode: CMR,RTCM,...
        if (!m_topcon_useAIMMode && !m_JAVAD_rtk_format.empty())
            JAVAD_sendMessage(format(
                                  "%%%%set,/par%s/imode,%s\r\n",
                                  m_JAVAD_rtk_src_port.c_str(),
                                  m_JAVAD_rtk_format.c_str())
                                  .c_str());
    }

    // Start NMEA messaging:
    //      JAVAD_sendMessage("%%em,,/msg/nmea/GGA:0.2\r\n");
    //      JAVAD_sendMessage("%%em,,/msg/nmea/RMC:0.2\r\n");
    // JAVAD_sendMessage("%%em,,/msg/jps/PS:0.2\r\n");

    if (m_topcon_useAIMMode)
    {
        if (m_verbose) cout << "[CGPSInterface] Using Advanced Input Mode";
        m_topcon_AIMConfigured = setJAVAD_AIM_mode();
        if (m_verbose) cout << "... done" << endl;
    }
    JAVAD_sendMessage(
        format("%%%%em,,/msg/nmea/GGA:%.1f\r\n", m_topcon_data_period).c_str());
    JAVAD_sendMessage(
        format("%%%%em,,/msg/nmea/RMC:%.1f\r\n", m_topcon_data_period)
            .c_str());  // FAMD: 10 Hz

    if (m_topcon_useAIMMode)
    {
        if (m_verbose)
            cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
                    "commands sent successfully with AIM."
                 << endl;
    }
    else
    {
        if (m_verbose)
            cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
                    "commands sent successfully."
                 << endl;
    }

    return true;
}

/** Send a custom data block to the GNSS device right now. Can be used to change
 * its behavior online as needed. */
bool CGPSInterface::sendCustomCommand(const void* data, const size_t datalen)
{
    try
    {
        std::lock_guard<std::mutex> lock(*m_data_stream_cs);
        m_data_stream->Write(data, datalen);
        return true;
    }
    catch (const std::exception& e)
    {
        std::cerr << "[CGPSInterface::sendCustomCommand] Error sending cmd: "
                  << e.what() << std::endl;
        return false;
    }
}