From 36a4129b1a604fa52bfb8fdf3b24f155c7ba45ba Mon Sep 17 00:00:00 2001
From: Cillian O'Driscoll <cillian.odriscoll@gmail.com>
Date: Wed, 18 Nov 2015 14:46:06 +0000
Subject: [PATCH] Added handling of PREAMBLE and RXTIME_SET messages

In the PRS Double Estimator tracking strategy.
---
 .../galileo_e1_prs_de_tracking_cc.cc          | 57 ++++++++++++++++++-
 .../galileo_e1_prs_de_tracking_cc.h           |  7 +++
 2 files changed, 62 insertions(+), 2 deletions(-)

diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.cc
index 15dc53491..c8a5b96f0 100644
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.cc
@@ -288,6 +288,10 @@ galileo_e1_prs_de_tracking_cc::galileo_e1_prs_de_tracking_cc(
     d_carrier_doppler_hz_prs = 0.0;
     d_acc_carrier_phase_rad = 0.0;
     d_acc_code_phase_secs = 0.0;
+
+    d_tow_received = false;
+    d_rx_time_set = false;
+    d_preamble_start_detected = false;
 }
 
 void galileo_e1_prs_de_tracking_cc::start_tracking()
@@ -344,6 +348,8 @@ void galileo_e1_prs_de_tracking_cc::start_tracking()
     d_enable_tracking = true;
     d_code_locked = false;
     d_carrier_locked = false;
+    d_cn0_estimation_counter = 0;
+    d_outage_detected = false;
 
     LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
               << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
@@ -1299,12 +1305,38 @@ void galileo_e1_prs_de_tracking_cc::start_tracking_prs()
         return;
     }
     // Initialise the code/phase and subcarrier estimates:
+    // Two possibilities:
+    // 1) We have decoded TOW from the nav message
+    // 2) We have rx time and a preamble in which case we can estimate TOW:
+    double timestamp_last_tow = d_timestamp_last_tow;
+    double last_tow = d_last_tow;
+
+    if( d_tow_received )
+    {
+        timestamp_last_tow = d_timestamp_last_tow;
+        last_tow = d_last_tow;
+
+    }
+    else if( d_rx_time_set && d_preamble_start_detected )
+    {
+        double t_preamble = std::floor( ( d_tow_rx_time +
+                    ( d_preamble_timestamp - d_timestamp_rx_time ) ) + 0.5 );
+
+        last_tow = t_preamble;
+        timestamp_last_tow = d_preamble_timestamp;
+    }
+    else
+    {
+        DLOG(INFO) << "Attempted to initialise PRS tracking with insufficient timing information. Bailing.";
+        return;
+    }
+
     double time_since_tow = static_cast< double >( d_sample_counter ) /
-        static_cast<double>( d_fs_in ) - d_timestamp_last_tow;
+        static_cast<double>( d_fs_in ) - timestamp_last_tow;
 
     double code_periods_since_tow = std::floor( time_since_tow / Galileo_E1_CODE_PERIOD + 0.5);
 
-    double curr_tow = d_last_tow + code_periods_since_tow*Galileo_E1_CODE_PERIOD +
+    double curr_tow = last_tow + code_periods_since_tow*Galileo_E1_CODE_PERIOD +
         //std::fmod( d_code_phase_chips, Galileo_E1_B_CODE_LENGTH_CHIPS ) / Galileo_E1_CODE_CHIP_RATE_HZ;
         d_rem_code_phase_samples / static_cast<double>( d_fs_in );
 
@@ -1394,6 +1426,27 @@ void galileo_e1_prs_de_tracking_cc::handle_gnss_message( pmt::pmt_t msg )
         }
     }
 
+    if( gnss_message::get_message( msg ) == "RECEIVER_TIME_SET" )
+    {
+        d_rx_time_set = true;
+        d_tow_rx_time = pmt::to_double( pmt::dict_ref( msg, pmt::mp( "TOW" ), not_found ) );
+        d_timestamp_rx_time = gnss_message::get_timestamp( msg );
+
+        log_str << ". TOW: " << d_tow_rx_time;
+    }
+
+    if( gnss_message::get_message( msg ) == "PREAMBLE_START_DETECTED" )
+    {
+        d_preamble_start_detected = true;
+        d_preamble_timestamp = gnss_message::get_timestamp( msg );
+    }
+
+    if( !d_prs_tracking_enabled && ( d_preamble_start_detected && d_rx_time_set ) )
+    {
+        log_str << ". Enabling PRS tracking with 1 s ambiguity resolution";
+        start_tracking_prs();
+    }
+
     LOG(INFO) << log_str.str();
 
 
diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.h
index c0e7c6d86..dc2225173 100644
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.h
@@ -292,6 +292,13 @@ private:
     double d_last_tow;
     double d_timestamp_last_tow;
 
+    bool d_rx_time_set;
+    double d_tow_rx_time;
+    double d_timestamp_rx_time;
+
+    bool d_preamble_start_detected;
+    double d_preamble_timestamp;
+
     bool d_prs_tracking_enabled;