Added gnss_message class

This is to be used to pass messages between blocks using the GnuRadio
message passing API. The primary use case is for passing information
from the telemetry_decoder to the tracking block. This can help inform
the tracking block of parity failure, time synch and other messages
which may be useful in determining the tracking state.

src/algorithms/channel/adapters/CMakeLists.txt

@@ -26,6 +26,7 @@ include_directories(
      $(CMAKE_CURRENT_SOURCE_DIR)
      ${CMAKE_SOURCE_DIR}/src/core/system_parameters
      ${CMAKE_SOURCE_DIR}/src/core/interfaces
+     ${CMAKE_SOURCE_DIR}/src/core/libs
      ${CMAKE_SOURCE_DIR}/src/core/receiver
      ${CMAKE_SOURCE_DIR}/src/algorithms/channel/libs
      ${GLOG_INCLUDE_DIRS}

src/algorithms/channel/adapters/channel.cc

@@ -45,6 +45,7 @@
 #include "telemetry_decoder_interface.h"
 #include "configuration_interface.h"
 #include "gnss_flowgraph.h"
+#include "gnss_message.h"
 
 
 
@@ -103,6 +104,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
     connected_ = false;
     message_ = 0;
     gnss_signal_ = Gnss_Signal();
+    msg_ports_connected_ = false;
 }
 
 
@@ -135,6 +137,51 @@ void Channel::connect(gr::top_block_sptr top_block)
     DLOG(INFO) << "pass_through_ -> tracking";
     top_block->connect(trk_->get_right_block(), 0, nav_->get_left_block(), 0);
     DLOG(INFO) << "tracking -> telemetry_decoder";
+
+    // Attempt to connect message ports:
+    pmt::pmt_t nav_op_msg_ports = nav_->get_left_block()->message_ports_out();
+    // this is a vector of symbols: check to see if it contains GNSS_MESSAGE_PORT_ID
+    size_t len = pmt::length( nav_op_msg_ports );
+    bool has_gnss_message_port = false;
+    size_t i = 0;
+    for( i = 0; i < len; ++i )
+    {
+        if( pmt::vector_ref( nav_op_msg_ports, i ) == GNSS_MESSAGE_PORT_ID )
+        {
+            has_gnss_message_port = true;
+            break;
+        }
+    }
+
+    // Now check if the tracking loop can accept messages:
+    if( has_gnss_message_port )
+    {
+        has_gnss_message_port = false;
+        pmt::pmt_t trk_ip_msg_ports = trk_->get_left_block()->message_ports_in();
+
+        len = pmt::length( trk_ip_msg_ports );
+        for( size_t j = 0; j < len; ++j )
+        {
+            if( pmt::vector_ref( trk_ip_msg_ports, j ) == GNSS_MESSAGE_PORT_ID )
+            {
+                // Connect the input port to the output port:
+                top_block->msg_connect( nav_->get_right_block(), GNSS_MESSAGE_PORT_ID,
+                                        trk_->get_left_block(), GNSS_MESSAGE_PORT_ID );
+
+                DLOG(INFO) << "Connected gnss_message port: telemetry_decoder -> tracking";
+                has_gnss_message_port = true;
+                msg_ports_connected_ = true;
+                break;
+
+            }
+        }
+    }
+
+    if( !msg_ports_connected_ )
+    {
+        DLOG(INFO) << "No gnss_message ports to connect";
+    }
+
     connected_ = true;
 }
 
@@ -150,6 +197,12 @@ void Channel::disconnect(gr::top_block_sptr top_block)
     top_block->disconnect(pass_through_->get_right_block(), 0, acq_->get_left_block(), 0);
     top_block->disconnect(pass_through_->get_right_block(), 0, trk_->get_left_block(), 0);
     top_block->disconnect(trk_->get_right_block(), 0, nav_->get_left_block(), 0);
+
+    if( msg_ports_connected_ )
+    {
+        top_block->msg_disconnect( nav_->get_right_block(), GNSS_MESSAGE_PORT_ID,
+                               trk_->get_left_block(), GNSS_MESSAGE_PORT_ID );
+    }
     pass_through_->disconnect(top_block);
     acq_->disconnect(top_block);
     trk_->disconnect(top_block);

src/algorithms/channel/adapters/channel.h

@@ -108,6 +108,7 @@ private:
     boost::shared_ptr<gr::msg_queue> queue_;
     concurrent_queue<int> channel_internal_queue_;
     boost::thread ch_thread_;
+    bool msg_ports_connected_;
     void run();
     void process_channel_messages();
 };

src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt

@@ -28,6 +28,7 @@ include_directories(
      $(CMAKE_CURRENT_SOURCE_DIR)
      ${CMAKE_SOURCE_DIR}/src/core/system_parameters
      ${CMAKE_SOURCE_DIR}/src/core/receiver
+     ${CMAKE_SOURCE_DIR}/src/core/libs
      ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs
      ${GLOG_INCLUDE_DIRS}
      ${GFlags_INCLUDE_DIRS}

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc

@@ -42,6 +42,7 @@
 #include <glog/logging.h>
 #include "control_message_factory.h"
 #include "gnss_synchro.h"
+#include "gnss_message.h"
 
 #ifndef _rotl
 #define _rotl(X,N)  ((X << N) ^ (X >> (32-N)))  // Used in the parity check algorithm
@@ -79,6 +80,9 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
         gr::block("gps_navigation_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
         gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
 {
+    // Create the gnss_message port:
+    message_port_register_out( GNSS_MESSAGE_PORT_ID );
+
     // initialize internal vars
     d_queue = queue;
     d_dump = dump;
@@ -219,6 +223,11 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
 
                             if (!d_flag_frame_sync)
                                 {
+                                    pmt::pmt_t msg = gnss_message::make( "PREAMBLE_START_DETECTED",
+                                            d_preamble_time_seconds );
+
+                                    message_port_pub( GNSS_MESSAGE_PORT_ID, msg );
+
                                     d_flag_frame_sync = true;
                                     if (corr_value < 0)
                                         {
@@ -241,6 +250,11 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
                     preamble_diff = d_sample_counter - d_preamble_index;
                     if (preamble_diff > 6001)
                         {
+                            pmt::pmt_t msg = gnss_message::make( "FRAME_SYNC_LOST",
+                                    in[0][0].Tracking_timestamp_secs );
+
+                            message_port_pub( GNSS_MESSAGE_PORT_ID, msg );
+
                             LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff;
                             d_stat = 0; //lost of frame sync
                             d_flag_frame_sync = false;
@@ -316,10 +330,21 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
         // TOW, in GPS, is referred to the START of the SUBFRAME, that is, THE FIRST SYMBOL OF THAT SUBFRAME, NOT THE PREAMBLE.
         // thus, no correction should be done. d_TOW_at_Preamble should be renamed to d_TOW_at_subframe_start.
         // Sice we detected the preable, then, we are in the last symbol of that preamble, or just at the start of the first subframe symbol.
+        // COD: 28/10/2015: In fact the 1st symbol of the preamble is the first symbol of the subframe.
+        // The issue here is that we refer everything back to in[0] which is the oldest
+        // input in the history. When d_flag_preamble is true it means that in[0]
+        // is the first symbol in the preamble.
         {
             d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + GPS_SUBFRAME_SECONDS; //we decoded the current TOW when the last word of the subframe arrive, so, we have a lag of ONE SUBFRAME
             d_TOW_at_current_symbol = d_TOW_at_Preamble;
             Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
+            pmt::pmt_t msg = gnss_message::make( "TOW_ACQUIRED",
+                    in[0][0].Tracking_timestamp_secs );
+
+            msg = pmt::dict_add( msg, pmt::mp("TOW"),
+                                        pmt::from_double( d_TOW_at_current_symbol ) );
+
+            message_port_pub( GNSS_MESSAGE_PORT_ID, msg );
             if (flag_TOW_set == false)
                 {
                     flag_TOW_set = true;

src/algorithms/tracking/adapters/CMakeLists.txt

@@ -40,6 +40,7 @@ include_directories(
      $(CMAKE_CURRENT_SOURCE_DIR)
      ${CMAKE_SOURCE_DIR}/src/core/system_parameters
      ${CMAKE_SOURCE_DIR}/src/core/interfaces
+     ${CMAKE_SOURCE_DIR}/src/core/libs
      ${CMAKE_SOURCE_DIR}/src/core/receiver
      ${CMAKE_SOURCE_DIR}/src/algorithms/tracking/gnuradio_blocks
      ${CMAKE_SOURCE_DIR}/src/algorithms/tracking/libs

src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt

@@ -42,6 +42,7 @@ include_directories(
      $(CMAKE_CURRENT_SOURCE_DIR)
      ${CMAKE_SOURCE_DIR}/src/core/system_parameters
      ${CMAKE_SOURCE_DIR}/src/core/interfaces
+     ${CMAKE_SOURCE_DIR}/src/core/libs
      ${CMAKE_SOURCE_DIR}/src/core/receiver
      ${CMAKE_SOURCE_DIR}/src/algorithms/tracking/libs
      ${CMAKE_SOURCE_DIR}/src/algorithms/libs

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc

@@ -124,6 +124,11 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
         d_carrier_loop_filter( 0.001, pll_initial_bw_hz, pll_loop_order, false ),
         d_code_loop_filter( 0.001, dll_initial_bw_hz, dll_loop_order, false )
 {
+    // Create the gnss_message input port
+    message_port_register_in( GNSS_MESSAGE_PORT_ID );
+    set_msg_handler( GNSS_MESSAGE_PORT_ID,
+            boost::bind( &Gps_L1_Ca_Dll_Pll_Tracking_cc::handle_gnss_message, this, _1 ) );
+
     // initialize internal vars
     d_queue = queue;
     d_dump = dump;
@@ -829,3 +834,23 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchr
 {
     d_acquisition_gnss_synchro = p_gnss_synchro;
 }
+
+
+void Gps_L1_Ca_Dll_Pll_Tracking_cc::handle_gnss_message( pmt::pmt_t msg )
+{
+    std::string telem_msg = gnss_message::get_message( msg );
+
+    std::stringstream log_str("");
+
+    log_str << "Received message " << telem_msg
+               << " with timestamp: " << gnss_message::get_timestamp( msg );
+
+    pmt::pmt_t not_found;
+
+    if( gnss_message::get_message( msg ) == "TOW_ACQUIRED" ){
+        log_str << ". TOW: " << pmt::to_double( pmt::dict_ref( msg, pmt::mp( "TOW" ), not_found ) );
+    }
+
+    LOG(INFO) << log_str.str();
+
+}

src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h

@@ -50,6 +50,7 @@
 #include "gnss_synchro.h"
 #include "tracking_loop_filter.h"
 #include "correlator.h"
+#include "gnss_message.h"
 
 class Gps_L1_Ca_Dll_Pll_Tracking_cc;
 
@@ -218,6 +219,9 @@ private:
 
     std::map<std::string, std::string> systemName;
     std::string sys;
+
+    // Handler for gnss_messages:
+    void handle_gnss_message( pmt::pmt_t msg );
 };
 
 #endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H

src/core/libs/CMakeLists.txt

@@ -26,6 +26,7 @@ set(CORE_LIBS_SOURCES
     ini.cc 
     INIReader.cc 
     string_converter.cc
+    gnss_message.cc
     gnss_sdr_supl_client.cc
 )
 	

src/core/libs/gnss_message.cc

@@ -0,0 +1,105 @@
+/*!
+ * \file gnss_message.cc
+ * \brief Implementation for a generic GNSS message PMT for use with the GnuRadio
+ * message passing API.
+ * \author Cillian O'Driscoll, 2015. cillian.odriscoll(@)gmail.com
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "gnss_message.h"
+
+bool gnss_message::is_valid( const pmt::pmt_t &msg )
+{
+
+    if( !pmt::is_dict( msg ) ){
+        return false;
+    }
+
+    pmt::pmt_t wrk;
+    pmt::pmt_t not_found;
+    wrk = pmt::dict_ref( msg, pmt::mp( "message" ), not_found );
+
+    if( wrk == not_found )
+    {
+        return false;
+    }
+
+    wrk = pmt::dict_ref( msg, pmt::mp( "timestamp" ), not_found );
+
+    if( wrk == not_found )
+    {
+        return false;
+    }
+
+    return true;
+}
+
+
+std::string gnss_message::get_message( const pmt::pmt_t &msg )
+{
+    if( !gnss_message::is_valid( msg ) )
+    {
+        std::string errMsg("Error: gnss_message ");
+        errMsg += pmt::symbol_to_string( msg ) + " is not a valid gnss_message";
+
+        throw errMsg;
+    }
+
+    pmt::pmt_t the_string;
+    pmt::pmt_t not_found;
+    the_string = pmt::dict_ref( msg, pmt::mp( "message" ), not_found );
+
+    return pmt::symbol_to_string( the_string );
+}
+
+double gnss_message::get_timestamp( const pmt::pmt_t &msg )
+{
+    if( !gnss_message::is_valid( msg ) )
+    {
+        std::string errMsg("Error: gnss_message ");
+        errMsg += pmt::symbol_to_string( msg ) + " is not a valid gnss_message";
+
+        throw errMsg;
+    }
+
+    pmt::pmt_t the_timestamp;
+    pmt::pmt_t not_found;
+    the_timestamp = pmt::dict_ref( msg, pmt::mp( "timestamp" ), not_found );
+
+    return pmt::to_double( the_timestamp );
+}
+
+pmt::pmt_t gnss_message::make( const std::string &msg, double timestamp )
+{
+    pmt::pmt_t ret = pmt::make_dict();
+
+    ret = pmt::dict_add( ret, pmt::mp( "message" ), pmt::mp( msg ) );
+
+    ret = pmt::dict_add( ret, pmt::mp( "timestamp" ), pmt::from_double( timestamp ) );
+
+    return ret;
+}
+

src/core/libs/gnss_message.h

@@ -0,0 +1,49 @@
+/*!
+ * \file gnss_message.h
+ * \brief Definitions for a generic GNSS message PMT for use with the GnuRadio
+ * message passing API.
+ * \author Cillian O'Driscoll, 2015. cillian.odriscoll(@)gmail.com
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015  (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ *          Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * GNSS-SDR is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * GNSS-SDR 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_MESSAGE_H
+#define GNSS_MESSAGE_H
+
+#include <pmt/pmt.h>
+#include <string>
+
+#define GNSS_MESSAGE_PORT_ID pmt::mp( "gnss_messages" )
+
+namespace gnss_message {
+    std::string get_message( const pmt::pmt_t &msg );
+    double get_timestamp( const pmt::pmt_t &msg );
+    pmt::pmt_t make( const std::string &msg, double timestamp );
+
+    bool is_valid( const pmt::pmt_t &msg );
+
+}
+
+#endif