Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr.git into next

docs/doxygen/other/reference_docs.dox

@@ -40,7 +40,7 @@ Check the <a href="http://ec.europa.eu/enterprise/policies/satnav/galileo/index_
 <a href="http://www.esa.int/Our_Activities/Navigation/The_future_-_Galileo/What_is_Galileo" target="_blank">Galileo website of the European Space Agency</a>.
 There is a website with <a href="http://igs.org/mgex/Status_GAL.htm" target="_blank">Galileo constellation status information</a> from the International GNSS Service.
 
-\li Galileo E5, E6, and E1: European GNSS (Galileo) Open Service.  <a href="http://ec.europa.eu/enterprise/policies/satnav/galileo/files/galileo-os-sis-icd-issue1-revision1_en.pdf" target="_blank"><b>Signal In Space Interface Control Document. Ref: OS SIS ICD, Issue 1.1</b></a>, European Commission, Sept. 2010. 
+\li Galileo E5, E6, and E1: European GNSS (Galileo) Open Service.  <a href="http://www.gsc-europa.eu/system/files/galileo_documents/Galileo_OS_SIS_ICD.pdf" target="_blank"><b>Signal In Space Interface Control Document. Ref: OS SIS ICD, Issue 1.2</b></a>, European Commission, Nov. 2015. 
 
 \li European GNSS (Galileo) Open Service  <a href="" target="_blank"><b>Signal-In-Space Operational Status Definition</b></a>, European Commission, Sept. 2015.
 

src/core/system_parameters/rtcm.cc

@@ -30,9 +30,11 @@
 
 #include "rtcm.h"
 #include <algorithm>  // for std::reverse
+#include <chrono>     // std::chrono::seconds
 #include <cmath>      // for std::fmod
 #include <cstdlib>    // for strtol
 #include <sstream>    // for std::stringstream
+#include <thread>
 #include <boost/algorithm/string.hpp>  // for to_upper_copy
 #include <boost/dynamic_bitset.hpp>
 #include <gflags/gflags.h>
@@ -41,7 +43,12 @@
 using google::LogMessage;
 
 DEFINE_int32(RTCM_Ref_Station_ID, 1234, "Reference Station ID in RTCM messages");
+DEFINE_int32(RTCM_Port, 2101 , "TCP port of the RTCM message server");
+// 2101 is the standard RTCM port according to the Internet Assigned Numbers Authority (IANA)
+// https://www.iana.org/assignments/service-names-port-numbers/service-names-port-numbers.xml
 
+DEFINE_string(Remote_RTCM_Server, "localhost", "Remote RTCM server address");
+DEFINE_int32(Remote_RTCM_Port, 2101 , "Remote TCP port of the RTCM message server");
 
 
 Rtcm::Rtcm()
@@ -49,9 +56,100 @@ Rtcm::Rtcm()
     Rtcm::reset_data_fields();
     preamble = std::bitset<8>("11010011");
     reserved_field = std::bitset<6>("000000");
+    rtcm_message_queue = std::make_shared< concurrent_queue<std::string> >();
+    // for each server, do:
+    boost::asio::ip::tcp::endpoint endpoint(boost::asio::ip::tcp::v4(), FLAGS_RTCM_Port);
+    servers.emplace_back(io_service, endpoint);
+    server_is_running = false;
 }
 
 
+Rtcm::~Rtcm()
+{}
+
+
+
+// *****************************************************************************************************
+//
+//   TCP Server / Client helper classes
+//
+// *****************************************************************************************************
+void Rtcm::run_server()
+{
+    std::cout << "Starting a TCP Server on port " << FLAGS_RTCM_Port << std::endl;
+    try
+    {
+            std::thread tq([&]{ std::make_shared<Queue_Reader>(io_service, rtcm_message_queue, FLAGS_RTCM_Port)->do_read_queue(); });
+            tq.detach();
+
+            std::thread t([&]{ io_service.run(); });
+            server_is_running = true;
+            t.detach();
+    }
+    catch (std::exception& e)
+    {
+            std::cerr << "Exception: " << e.what() << "\n";
+    }
+}
+
+
+void Rtcm::stop_service()
+{
+    io_service.stop();
+}
+
+
+void Rtcm::stop_server()
+{
+    std::cout << "Stopping TCP Server on port " << FLAGS_RTCM_Port << std::endl;
+    rtcm_message_queue->push("Goodbye"); // this terminates tq
+    Rtcm::stop_service();
+    servers.front().close_server();
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    server_is_running = false;
+}
+
+
+void Rtcm::run_client()
+{
+    std::cout << "Starting a TCP Client on port " << FLAGS_Remote_RTCM_Port << std::endl;
+    std::string remote_host = FLAGS_Remote_RTCM_Server;
+    std::string remote_port = std::to_string(FLAGS_Remote_RTCM_Port);
+    boost::asio::ip::tcp::resolver resolver(io_service);
+    auto endpoint_iterator = resolver.resolve({ remote_host.c_str(), remote_port.c_str() });
+
+    clients.emplace_back(io_service, endpoint_iterator);
+    try
+    {
+            std::thread t([&](){ io_service.run(); });
+            t.detach();
+    }
+    catch (std::exception& e)
+    {
+            std::cerr << "Exception: " << e.what() << "\n";
+    }
+}
+
+
+void Rtcm::stop_client()
+{
+    std::cout << "Stopping TCP Client on port " << FLAGS_Remote_RTCM_Port << std::endl;
+    clients.front().close();
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    Rtcm::stop_service();
+}
+
+
+void Rtcm::send_message(const std::string & msg)
+{
+    rtcm_message_queue->push(msg);
+}
+
+
+bool Rtcm::is_server_running()
+{
+    return server_is_running;
+}
 
 
 // *****************************************************************************************************

src/core/system_parameters/rtcm.h

@@ -34,12 +34,18 @@
 
 
 #include <bitset>
+#include <deque>
 #include <map>
+#include <memory>
+#include <set>
 #include <string>
+#include <thread>
 #include <utility>
 #include <vector>
+#include <boost/asio.hpp>
 #include <boost/crc.hpp>
 #include <boost/date_time/posix_time/posix_time.hpp>
+#include "concurrent_queue.h"
 #include "gnss_synchro.h"
 #include "galileo_fnav_message.h"
 #include "gps_navigation_message.h"
@@ -79,6 +85,7 @@ class Rtcm
 {
 public:
     Rtcm(); //<! Default constructor
+    ~Rtcm();
 
     /*!
      * \brief Prints message type 1001 (L1-Only GPS RTK Observables)
@@ -243,12 +250,12 @@ public:
             bool divergence_free,
             bool more_messages);
 
-    unsigned int lock_time(const Gps_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro); //<! Returns the time period in which GPS L1 signals have been continually tracked.
+    unsigned int lock_time(const Gps_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro);      //<! Returns the time period in which GPS L1 signals have been continually tracked.
     unsigned int lock_time(const Gps_CNAV_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro); //<! Returns the time period in which GPS L2 signals have been continually tracked.
-    unsigned int lock_time(const Galileo_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro); //<! Returns the time period in which Galileo signals have been continually tracked.
+    unsigned int lock_time(const Galileo_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro);  //<! Returns the time period in which Galileo signals have been continually tracked.
 
-    std::string bin_to_hex(const std::string& s); //<! Returns a string of hexadecimal symbols from a string of binary symbols
-    std::string hex_to_bin(const std::string& s); //<! Returns a string of binary symbols from a string of hexadecimal symbols
+    std::string bin_to_hex(const std::string& s);        //<! Returns a string of hexadecimal symbols from a string of binary symbols
+    std::string hex_to_bin(const std::string& s);        //<! Returns a string of binary symbols from a string of hexadecimal symbols
 
     unsigned long int bin_to_uint(const std::string& s); //<! Returns an unsigned long int from a string of binary symbols
     long int bin_to_int(const std::string& s);           //<! Returns a long int from a string of binary symbols
@@ -261,6 +268,15 @@ public:
 
     bool check_CRC(const std::string & message);         //<! Checks that the CRC of a RTCM package is correct
 
+    void run_server();                                   //<! Starts running the server
+    void stop_server();                                  //<! Stops the server
+
+    void run_client();                                   //<! Starts running the client
+    void stop_client();                                  //<! Stops the client
+
+    void send_message(const std::string & message);      //<! Sends a message through the server
+    bool is_server_running();                            //<! Returns true if the server is running, false otherwise
+
 private:
     //
     // Generation of messages content
@@ -324,6 +340,487 @@ private:
     unsigned int msm_lock_time_indicator(unsigned int lock_time_period_s);
     unsigned int msm_extended_lock_time_indicator(unsigned int lock_time_period_s);
 
+    //
+    // Classes for TCP communication
+    //
+    class Rtcm_Message
+    {
+    public:
+        enum { header_length = 6 };
+        enum { max_body_length = 1029 };
+
+        Rtcm_Message()
+        : body_length_(0)
+        { }
+
+        const char* data() const
+        {
+            return data_;
+        }
+
+        char* data()
+        {
+            return data_;
+        }
+
+        std::size_t length() const
+        {
+            return header_length + body_length_;
+        }
+
+        const char* body() const
+        {
+            return data_ + header_length;
+        }
+
+        char* body()
+        {
+            return data_ + header_length;
+        }
+
+        std::size_t body_length() const
+        {
+            return body_length_;
+        }
+
+        void body_length(std::size_t new_length)
+        {
+            body_length_ = new_length;
+            if (body_length_ > max_body_length)
+                body_length_ = max_body_length;
+        }
+
+        bool decode_header()
+        {
+            char header[header_length + 1] = "";
+            std::strncat(header, data_, header_length);
+            if(header[0] != 'G' || header[1] != 'S')
+                {
+                    return false;
+                }
+
+            char header2_[header_length - 1] = "";
+            std::strncat(header2_, data_ + 2 , header_length - 2);
+            body_length_ = std::atoi(header2_);
+            if(body_length_ == 0)
+                {
+                    return false;
+                }
+
+            if (body_length_ > max_body_length)
+                {
+                    body_length_ = 0;
+                    return false;
+                }
+            return true;
+        }
+
+        void encode_header()
+        {
+            char header[header_length + 1] = "";
+            std::sprintf(header, "GS%4d", static_cast<int>(body_length_));
+            std::memcpy(data_, header, header_length);
+        }
+
+    private:
+        char data_[header_length + max_body_length];
+        std::size_t body_length_;
+    };
+
+
+    class Rtcm_Listener
+    {
+    public:
+        virtual ~Rtcm_Listener() {}
+        virtual void deliver(const Rtcm_Message & msg) = 0;
+    };
+
+
+    class Rtcm_Listener_Room
+    {
+    public:
+        void join(std::shared_ptr<Rtcm_Listener> participant)
+        {
+            participants_.insert(participant);
+            for (auto msg: recent_msgs_)
+                participant->deliver(msg);
+        }
+
+        void leave(std::shared_ptr<Rtcm_Listener> participant)
+        {
+            participants_.erase(participant);
+        }
+
+        void deliver(const Rtcm_Message & msg)
+        {
+            recent_msgs_.push_back(msg);
+            while (recent_msgs_.size() > max_recent_msgs)
+                recent_msgs_.pop_front();
+
+            for (auto participant: participants_)
+                participant->deliver(msg);
+        }
+
+    private:
+        std::set<std::shared_ptr<Rtcm_Listener> > participants_;
+        enum { max_recent_msgs = 1 };
+        std::deque<Rtcm_Message> recent_msgs_;
+    };
+
+
+    class Rtcm_Session
+            : public Rtcm_Listener,
+              public std::enable_shared_from_this<Rtcm_Session>
+    {
+    public:
+        Rtcm_Session(boost::asio::ip::tcp::socket socket, Rtcm_Listener_Room & room) : socket_(std::move(socket)), room_(room)   { }
+
+        void start()
+        {
+            room_.join(shared_from_this());
+            do_read_message_header();
+        }
+
+        void deliver(const Rtcm_Message & msg)
+        {
+            bool write_in_progress = !write_msgs_.empty();
+            write_msgs_.push_back(msg);
+            if (!write_in_progress)
+                {
+                    do_write();
+                }
+        }
+
+    private:
+        void do_read_message_header()
+        {
+            auto self(shared_from_this());
+            boost::asio::async_read(socket_,
+                    boost::asio::buffer(read_msg_.data(), Rtcm_Message::header_length),
+                    [this, self](boost::system::error_code ec, std::size_t /*length*/)
+                    {
+                if (!ec && read_msg_.decode_header())
+                    {
+                        do_read_message_body();
+                    }
+                else
+                    {
+                        std::cout << "Closing connection with client from " << socket_.remote_endpoint().address() << std::endl;
+                        room_.leave(shared_from_this());
+                    }
+                    });
+        }
+
+        void do_read_message_body()
+        {
+            auto self(shared_from_this());
+            boost::asio::async_read(socket_,
+                    boost::asio::buffer(read_msg_.body(), read_msg_.body_length()),
+                    [this, self](boost::system::error_code ec, std::size_t /*length*/)
+                    {
+                if (!ec)
+                    {
+                        room_.deliver(read_msg_);
+                        //std::cout << "Delivered message (session): ";
+                        //std::cout.write(read_msg_.body(), read_msg_.body_length());
+                        //std::cout << std::endl;
+                        do_read_message_header();
+                    }
+                else
+                    {
+                        std::cout << "Closing connection with client from " << socket_.remote_endpoint().address() << std::endl;
+                        room_.leave(shared_from_this());
+                    }
+                    });
+        }
+
+        void do_write()
+        {
+            auto self(shared_from_this());
+            boost::asio::async_write(socket_,
+                    boost::asio::buffer(write_msgs_.front().body(),
+                            write_msgs_.front().body_length()), [this, self](boost::system::error_code ec, std::size_t /*length*/)
+                            {
+                if(!ec)
+                    {
+                        write_msgs_.pop_front();
+                        if(!write_msgs_.empty())
+                            {
+                                do_write();
+                            }
+                    }
+                else
+                    {
+                        std::cout << "Closing connection with client from " << socket_.remote_endpoint().address() << std::endl;
+                        room_.leave(shared_from_this());
+                    }
+                            });
+        }
+
+        boost::asio::ip::tcp::socket socket_;
+        Rtcm_Listener_Room & room_;
+        Rtcm_Message read_msg_;
+        std::deque<Rtcm_Message> write_msgs_;
+    };
+
+
+    class Tcp_Internal_Client
+            : public std::enable_shared_from_this<Tcp_Internal_Client>
+    {
+    public:
+        Tcp_Internal_Client(boost::asio::io_service& io_service,
+                boost::asio::ip::tcp::resolver::iterator endpoint_iterator)
+    : io_service_(io_service), socket_(io_service)
+    {
+            do_connect(endpoint_iterator);
+    }
+
+        void close()
+        {
+            io_service_.post([this]() { socket_.close(); });
+        }
+
+        void write(const Rtcm_Message & msg)
+        {
+            io_service_.post(
+                    [this, msg]()
+                    {
+                bool write_in_progress = !write_msgs_.empty();
+                write_msgs_.push_back(msg);
+                if (!write_in_progress)
+                    {
+                        do_write();
+                    }
+                    });
+        }
+
+    private:
+        void do_connect(boost::asio::ip::tcp::resolver::iterator endpoint_iterator)
+        {
+            boost::asio::async_connect(socket_, endpoint_iterator,
+                    [this](boost::system::error_code ec, boost::asio::ip::tcp::resolver::iterator)
+                    {
+                if (!ec)
+                    {
+                        do_read_message();
+                    }
+                else
+                    {
+                        std::cout << "Server is down." << std::endl;
+                    }
+                    });
+        }
+
+        void do_read_message()
+        {
+            boost::asio::async_read(socket_,
+                    boost::asio::buffer(read_msg_.data(), 1029),
+                    [this](boost::system::error_code ec, std::size_t /*length*/)
+                    {
+                if (!ec )
+                    {
+                        do_read_message();
+                    }
+                else
+                    {
+                        std::cout << "Error in client" << std::endl;
+                        socket_.close();
+                    }
+                    });
+        }
+
+        void do_write()
+        {
+
+            boost::asio::async_write(socket_,
+                    boost::asio::buffer(write_msgs_.front().data(), write_msgs_.front().length()),
+                    [this](boost::system::error_code ec, std::size_t /*length*/)
+                    {
+                if (!ec)
+                    {
+                        write_msgs_.pop_front();
+                        if (!write_msgs_.empty())
+                            {
+                                do_write();
+                            }
+                    }
+                else
+                    {
+                        socket_.close();
+                    }
+                    });
+        }
+
+        boost::asio::io_service& io_service_;
+        boost::asio::ip::tcp::socket socket_;
+        Rtcm_Message read_msg_;
+        std::deque<Rtcm_Message> write_msgs_;
+    };
+
+
+    class Queue_Reader
+    {
+    public:
+        Queue_Reader(boost::asio::io_service& io_service, std::shared_ptr< concurrent_queue<std::string> > & queue, int port) : queue_(queue)
+    {
+            boost::asio::ip::tcp::resolver resolver(io_service);
+            std::string host("localhost");
+            std::string port_str = std::to_string(port);
+            auto queue_endpoint_iterator = resolver.resolve({ host.c_str(), port_str.c_str() });
+            c = std::make_shared<Tcp_Internal_Client>(io_service, queue_endpoint_iterator);
+    }
+
+        void do_read_queue()
+        {
+            for(;;)
+                {
+                    std::string message;
+                    Rtcm_Message msg;
+                    queue_->wait_and_pop(message);
+                    if(message.compare("Goodbye") == 0) break;
+                    const char *char_msg = message.c_str();
+                    msg.body_length(message.length());
+                    std::memcpy(msg.body(), char_msg, msg.body_length());
+                    msg.encode_header();
+                    c->write(msg);
+                }
+        }
+    private:
+        std::shared_ptr<Tcp_Internal_Client> c;
+        std::shared_ptr< concurrent_queue<std::string> > & queue_;
+    };
+
+
+    class Tcp_Server
+    {
+    public:
+        Tcp_Server(boost::asio::io_service& io_service, const boost::asio::ip::tcp::endpoint& endpoint)
+    : io_service_(io_service), acceptor_(io_service), socket_(io_service)
+    {
+            acceptor_.open(endpoint.protocol());
+            acceptor_.set_option(boost::asio::ip::tcp::acceptor::reuse_address(true));
+            acceptor_.bind(endpoint);
+            acceptor_.listen();
+            do_accept();
+    }
+
+        void close_server()
+        {
+            socket_.close();
+            acceptor_.close();
+        }
+
+    private:
+        void do_accept()
+        {
+            acceptor_.async_accept(socket_, [this](boost::system::error_code ec)
+                    {
+                if (!ec)
+                    {
+                        if(first_client)
+                            {
+                                std::cout << "The TCP Server is up and running. Accepting connections ..." << std::endl;
+                                first_client = false;
+                            }
+                        else
+                            {
+                                std::cout << "Starting RTCM TCP server session..." << std::endl;
+                                std::cout << "Serving client from " << socket_.remote_endpoint().address() << std::endl;
+                            }
+                        std::make_shared<Rtcm_Session>(std::move(socket_), room_)->start();
+                    }
+                else
+                    {
+                        std::cout << "Error when invoking a RTCM session. " << ec << std::endl;
+                    }
+                do_accept();
+                    });
+        }
+
+        boost::asio::io_service& io_service_;
+        boost::asio::ip::tcp::acceptor acceptor_;
+        boost::asio::ip::tcp::socket socket_;
+        Rtcm_Listener_Room room_;
+        bool first_client = true;
+    };
+
+
+    class Tcp_Client
+    {
+    public:
+        Tcp_Client(boost::asio::io_service& io_service,
+                boost::asio::ip::tcp::resolver::iterator endpoint_iterator)
+    : io_service_(io_service), socket_(io_service)
+    {
+            do_connect(endpoint_iterator);
+    }
+
+        void close()
+        {
+            io_service_.post([this]() { socket_.close(); });
+        }
+
+    private:
+        void do_connect(boost::asio::ip::tcp::resolver::iterator endpoint_iterator)
+        {
+            std::cout << "Connecting to server..." << std::endl;
+            boost::asio::async_connect(socket_, endpoint_iterator,
+                    [this](boost::system::error_code ec, boost::asio::ip::tcp::resolver::iterator)
+                    {
+                if (!ec)
+                    {
+                        std::cout << "Connected." << std::endl;
+                        do_read_message();
+                    }
+                else
+                    {
+                        std::cout << "Server is down." << std::endl;
+                    }
+                    });
+        }
+
+        void do_read_message()
+        {
+            // Waiting for data and reading forever, until connection is closed.
+            for(;;)
+                {
+                    std::array<char, 1029> buf;
+                    boost::system::error_code error;
+
+                    size_t len = socket_.read_some(boost::asio::buffer(buf), error);
+
+                    if(error == boost::asio::error::eof)
+                        break; // // Connection closed cleanly by peer.
+                    else if(error)
+                        {
+                            std::cout << "Error: " << error << std::endl;
+                            //socket_.close();
+                            break;
+                        }
+                    std::cout << "Received message: ";
+                    std::cout.write(buf.data(), len);
+                    std::cout << std::endl;
+                }
+
+            std::cout << std::endl;
+            socket_.close();
+            std::cout << "Connection closed by the server. Good bye." << std::endl;
+        }
+
+        boost::asio::io_service& io_service_;
+        boost::asio::ip::tcp::socket socket_;
+    };
+
+
+    boost::asio::io_service io_service;
+    std::shared_ptr< concurrent_queue<std::string> > rtcm_message_queue;
+    std::thread t;
+    std::thread tq;
+    std::list<Rtcm::Tcp_Server> servers;
+    std::list<Rtcm::Tcp_Client> clients;
+    bool server_is_running;
+    void stop_service();
+
     //
     // Transport Layer
     //

src/tests/formats/rtcm_test.cc

@@ -30,6 +30,7 @@
  */
 
 #include <memory>
+#include <thread>
 #include "rtcm.h"
 
 TEST(Rtcm_Test, Hex_to_bin)
@@ -519,3 +520,41 @@ TEST(Rtcm_Test, MSM1)
     EXPECT_EQ(psrng4_s, read_psrng4_s_2);
 }
 
+
+TEST(Rtcm_Test, InstantiateServer)
+{
+    auto rtcm = std::make_shared<Rtcm>();
+    rtcm->run_server();
+    std::string msg("Hello");
+    rtcm->send_message(msg);
+    std::string test3 = "ff";
+    std::string test3_bin = rtcm->hex_to_bin(test3);
+    EXPECT_EQ(0, test3_bin.compare("11111111"));
+    rtcm->stop_server();
+    std::string test6 = "0011";
+    std::string test6_hex = rtcm->bin_to_hex(test6);
+    EXPECT_EQ(0, test6_hex.compare("3"));
+    long unsigned int expected1 = 42;
+    EXPECT_EQ(expected1, rtcm->bin_to_uint("00101010"));
+    rtcm->run_server();
+    std::string test4_bin = rtcm->hex_to_bin(test3);
+    std::string s("Testing");
+    rtcm->send_message(s);
+    rtcm->stop_server();
+    EXPECT_EQ(0, test4_bin.compare("11111111"));
+}
+
+/*
+TEST(Rtcm_Test, InstantiateClient)
+{
+    auto rtcm = std::make_shared<Rtcm>();
+    rtcm->run_client();
+    std::string test3 = "ff";
+    std::string test3_bin = rtcm->hex_to_bin(test3);
+    EXPECT_EQ(0, test3_bin.compare("11111111"));
+    rtcm->stop_client();
+    std::string test3_bin2 = rtcm->hex_to_bin(test3);
+    EXPECT_EQ(0, test3_bin2.compare("11111111"));
+} */
+
+