CNTRIPEmitter.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
 
#include "hwdrivers-precomp.h"   // Precompiled headers
 
#include <mrpt/hwdrivers/CNTRIPEmitter.h>
 
#include <mrpt/system/filesystem.h>
#include <mrpt/system/string_utils.h>
 
IMPLEMENTS_GENERIC_SENSOR(CNTRIPEmitter,mrpt::hwdrivers)
 
using namespace std;
using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::obs;
using namespace mrpt::hwdrivers;
 
/*-------------------------------------------------------------
                                                CNTRIPEmitter
-------------------------------------------------------------*/
CNTRIPEmitter::CNTRIPEmitter() :
        m_client(),
        m_com_port(""),
        m_com_bauds(38400),
        m_transmit_to_server(true),
        m_rate_count(0)
{
 
}
 
/*-------------------------------------------------------------
                                                ~CNTRIPEmitter
-------------------------------------------------------------*/
CNTRIPEmitter::~CNTRIPEmitter()
{
        m_client.close();
        if (m_out_COM.isOpen()) m_out_COM.close();
        if (m_raw_output_file_stream.is_open()) m_raw_output_file_stream.close();
}
 
/*-------------------------------------------------------------
                                                doProcess
-------------------------------------------------------------*/
void CNTRIPEmitter::doProcess()
{
        vector_byte  buf;
        m_client.stream_data.readAndClear(buf);
 
        if (!buf.empty())
        {
                if (m_verbose) {
                        const double At = m_rate_timer.Tac();
                        m_rate_count += buf.size();
                        if (At > 5.0) {
                                const double estim_rate_Bps = m_rate_count / At;
                                cout << format("[NTRIP %s] Rate: %.02f B/s\n", mrpt::system::timeLocalToString(mrpt::system::now()).c_str(), estim_rate_Bps);
                                m_rate_timer.Tic();
                                m_rate_count = 0;
                        }
 
                        cout << format("[NTRIP %s] RX (%u bytes)\n", mrpt::system::timeLocalToString(mrpt::system::now()).c_str(), (unsigned int)buf.size());
                }
                if (m_out_COM.isOpen()) {
                        // Send through the serial port:
                        cout << format("[NTRIP %s] RX: %u bytes\n", mrpt::system::timeLocalToString(mrpt::system::now()).c_str(), (unsigned)buf.size());
                        m_out_COM.WriteBuffer(&buf[0], buf.size());
                }
 
                if (m_raw_output_file_stream.is_open()) {
                        m_raw_output_file_stream.write( reinterpret_cast<const char*>(&buf[0]), buf.size());
                }
        }
 
        // Try to read a msg from the receiver -> NTRIP caster
        if (m_transmit_to_server && m_out_COM.isOpen())
        {
                char rxbuf[50];
                const size_t nReadActual = m_out_COM.Read(rxbuf,sizeof(rxbuf)-1);
                if (nReadActual)
                {
                        rxbuf[nReadActual] = 0;
                        if (m_verbose) cout << format("[NTRIP %s] TX (%u bytes)\n", mrpt::system::timeLocalToString(mrpt::system::now()).c_str(), (unsigned int)nReadActual);
                }
        }
 
        mrpt::system::sleep(1);
}
 
/*-------------------------------------------------------------
                                                initialize
-------------------------------------------------------------*/
void CNTRIPEmitter::initialize()
{
        if (m_out_COM.isOpen()) m_out_COM.close();
 
        if (!m_com_port.empty())
        {
                cout << format("[NTRIP] Opening %s...\n",m_com_port.c_str() );
                m_out_COM.open(m_com_port);
                m_out_COM.setConfig(m_com_bauds);
                m_out_COM.setTimeouts(0,0,10,0,1);
                m_out_COM.purgeBuffers();
                cout << format("[NTRIP] Open %s Ok.\n",m_com_port.c_str() );
        }
 
        if (m_raw_output_file_stream.is_open()) {
                m_raw_output_file_stream.close();
        }
 
        if (!m_raw_output_file_prefix.empty())
        {
                const string fil = mrpt::system::fileNameStripInvalidChars(m_raw_output_file_prefix + mrpt::system::dateTimeLocalToString(mrpt::system::now()) + string(".bin"));
                m_raw_output_file_stream.open(fil.c_str(), std::ofstream::out | std::ofstream::binary);
                if (!m_raw_output_file_stream.is_open())
                        THROW_EXCEPTION_FMT("Error opening output raw file: `%s`",fil.c_str());
        }
 
        string errstr;
        if (!m_client.open(m_ntrip_args,errstr))
                THROW_EXCEPTION_FMT("ERROR trying to connect to NTRIP caster: %s",errstr.c_str());
}
 
 
/* -----------------------------------------------------
                loadConfig_sensorSpecific
   ----------------------------------------------------- */
void  CNTRIPEmitter::loadConfig_sensorSpecific(
        const mrpt::utils::CConfigFileBase &c,
        const std::string         &s )
{
#ifdef MRPT_OS_WINDOWS
        m_com_port = c.read_string(s, "COM_port_WIN", "");
#else
        m_com_port = c.read_string(s, "COM_port_LIN", "");
#endif
 
        m_raw_output_file_prefix = c.read_string(s, "raw_output_file_prefix","");
 
        ASSERTMSG_(!m_raw_output_file_prefix.empty() || !m_com_port.empty(), "At least one of either raw file output or serial COM file must be specified in configuration file!");
 
        if (!m_com_port.empty()) {
                m_com_bauds = c.read_int(s, "baudRate", m_com_bauds, true);
        }
 
        m_transmit_to_server = c.read_bool( s, "transmit_to_server",m_transmit_to_server);
 
        m_ntrip_args.mountpoint = mrpt::system::trim( c.read_string(s, "mountpoint","",true) );
        m_ntrip_args.server     = mrpt::system::trim( c.read_string(s, "server","",true) );
        m_ntrip_args.port       = c.read_int(s, "port",2101,true);
 
        m_ntrip_args.user = mrpt::system::trim( c.read_string(s, "user","") );
        m_ntrip_args.password = mrpt::system::trim( c.read_string(s, "password","") );
}