Fix GPS L5

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc

@@ -123,7 +123,7 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg)
                     gps_cnav_ephemeris = boost::any_cast<std::shared_ptr<Gps_CNAV_Ephemeris>>(pmt::any_ref(msg));
                     // update/insert new ephemeris record to the global ephemeris map
                     d_ls_pvt->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris;
-                    LOG(INFO) << "New GPS CNAV ephemeris record has arrived ";
+                    DLOG(INFO) << "New GPS CNAV ephemeris record has arrived ";
                 }
             else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Iono>) )
                 {
@@ -335,7 +335,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
     msgctl(sysv_msqid, IPC_RMID, NULL);
 
     //save GPS L2CM ephemeris to XML file
-    std::string file_name = "eph_GPS_L2CM.xml";
+    std::string file_name = "eph_GPS_L2CM_L5.xml";
 
     if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0)
         {
@@ -470,7 +470,8 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite
                             if(((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0))
                                     || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2S") == 0))
                                     || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1B") == 0))
-                                    || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0)))
+                                    || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0))
+                                    || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("L5") == 0)))
                                 {
                                     // store valid observables in a map.
                                     gnss_observables_map.insert(std::pair<int,Gnss_Synchro>(i, in[i][epoch]));

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -275,7 +275,7 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
                                 }
                         }
                     //GPS L5
-                    if(sig_.compare("5X") == 0)
+                    if(sig_.compare("L5") == 0)
                         {
                             gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
                             if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend())
@@ -346,7 +346,6 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
                     nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */
                     nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */
                 }
-
             result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data);
             if(result == 0)
                 {

src/algorithms/libs/gps_l5_signal.cc

@@ -186,7 +186,7 @@ void gps_l5i_code_gen_complex(std::complex<float>* _dest, unsigned int _prn)
 
     if (_prn > 0 and _prn < 51)
         {
-          make_l5i(_code, _prn);
+          make_l5i(_code, _prn - 1);
         }
 
     for (signed int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++)
@@ -206,7 +206,7 @@ void gps_l5i_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _
     int32_t* _code = new int32_t[GPS_L5i_CODE_LENGTH_CHIPS];
     if (_prn > 0 and _prn < 51)
         {
-          make_l5i(_code, _prn);
+          make_l5i(_code, _prn - 1);
         }
 
     signed int _samplesPerCode, _codeValueIndex;
@@ -253,7 +253,7 @@ void gps_l5q_code_gen_complex(std::complex<float>* _dest, unsigned int _prn)
 
     if (_prn > 0 and _prn < 51)
         {
-          make_l5q(_code, _prn);
+          make_l5q(_code, _prn - 1);
         }
 
     for (signed int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++)
@@ -273,7 +273,7 @@ void gps_l5q_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _
     int32_t* _code = new int32_t[GPS_L5q_CODE_LENGTH_CHIPS];
     if (_prn > 0 and _prn < 51)
         {
-          make_l5q(_code, _prn);
+          make_l5q(_code, _prn - 1);
         }
 
     signed int _samplesPerCode, _codeValueIndex;

src/algorithms/libs/rtklib/rtklib_pntpos.cc

@@ -97,7 +97,13 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
 
 
     /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */
-    if (NFREQ >= 3 && (sys & (SYS_GAL | SYS_SBS))) j = 2;
+    if (sys & (SYS_GAL | SYS_SBS)) {j = 2;}
+
+    if (sys == SYS_GPS)
+        {
+            if(obs->code[1] != CODE_NONE) {j = 1;}
+            else if(obs->code[2] != CODE_NONE) {j = 2;}
+        }
 
     if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0)
         {
@@ -132,7 +138,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
     P2_C2 = nav->cbias[obs->sat-1][2];
 
     /* if no P1-P2 DCB, use TGD instead */
-    if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS)))
+    if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) //CHECK!
         {
             P1_P2 = (1.0 - gamma_) * gettgd(obs->sat, nav);
         }

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc

@@ -170,12 +170,6 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__(
     current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol;
     current_synchro_data.Flag_valid_word = d_flag_valid_word;
 
-    //    if (flag_PLL_180_deg_phase_locked == true)
-    //        {
-    //            //correct the accumulated phase for the Costas loop phase shift, if required
-    //            current_synchro_data.Carrier_phase_rads += GPS_PI;
-    //        }
-
     if(d_dump == true)
         {
             // MULTIPLEXED FILE RECORDING - Record results to file
@@ -192,7 +186,7 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__(
             }
             catch (const std::ifstream::failure & e)
             {
-                    LOG(WARNING) << "Exception writing observables dump file " << e.what();
+                    LOG(WARNING) << "Exception writing Telemetry GPS L2 dump file " << e.what();
             }
         }
 
@@ -230,7 +224,7 @@ void gps_l2c_telemetry_decoder_cc::set_channel(int channel)
                     }
                     catch (const std::ifstream::failure &e)
                     {
-                            LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
+                            LOG(WARNING) << "channel " << d_channel << " Exception opening Telemetry GPS L2 dump file " << e.what();
                     }
                 }
         }

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc

@@ -62,14 +62,10 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc(
     d_dump = dump;
     d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
     DLOG(INFO) << "GPS L5 TELEMETRY PROCESSING: satellite " << d_satellite;
-    //set_output_multiple (1);
     d_channel = 0;
     d_flag_valid_word = false;
-    d_TOW_at_current_symbol = 0;
+    d_TOW_at_current_symbol = 0.0;
-    d_TOW_at_Preamble = 0;
+    d_TOW_at_Preamble = 0.0;
-    d_state = 0; //initial state
-    d_crc_error_count = 0;
-
     //initialize the CNAV frame decoder (libswiftcnav)
     cnav_msg_decoder_init(&d_cnav_decoder);
     for(int aux = 0; aux < GPS_L5_NH_CODE_LENGTH; aux++)
@@ -117,33 +113,30 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__((
     current_synchro_data = in[0];
     consume_each(1); //one by one
     sym_hist.push_back(in[0].Prompt_I);
-    double symbol_value = 0.0;
+    int corr_NH = 0;
+    int symbol_value = 0;
 
+    //Search correlation with Neuman-Hofman Code (see IS-GPS-705D)
     if(sym_hist.size() == GPS_L5_NH_CODE_LENGTH)
         {
-            std::deque<double>::iterator it;
-            int corr_NH = 0;
-            it = sym_hist.begin();
             for(int i = 0; i < GPS_L5_NH_CODE_LENGTH; i++)
                 {
-                    if((bits_NH[i] * (*it)) > 0.0){corr_NH += 1;}
+                    if((bits_NH[i] * sym_hist.at(i)) > 0.0) {corr_NH += 1;}
                     else {corr_NH -= 1;}
-                    it++;
                 }
-            if(abs(corr_NH) == GPS_L5_NH_CODE_LENGTH){sync_NH = true;}
+            if(abs(corr_NH) == GPS_L5_NH_CODE_LENGTH)
+                {
+                    sync_NH = true;
+                    if(corr_NH > 0) {symbol_value = 1;}
+                    else {symbol_value = -1;}
+                    new_sym = true;
+                    sym_hist.clear();
+                }
             else
                 {
                     sym_hist.pop_front();
                     sync_NH = false;
-                }
+                    new_sym = false;
-            if (sync_NH)
-                {
-                    new_sym = true;
-                    for(it = sym_hist.begin(); it != sym_hist.end(); it++)
-                        {
-                            symbol_value += (*it);
-                        }
-                    sym_hist.clear();
                 }
         }
 
@@ -154,7 +147,7 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__((
     //add the symbol to the decoder
     if(new_sym)
         {
-            u8 symbol_clip = static_cast<u8>(symbol_value > 0.0) * 255;
+            u8 symbol_clip = static_cast<u8>(symbol_value > 0) * 255;
             flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay);
             new_sym = false;
         }
@@ -162,7 +155,6 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__((
     //check if new CNAV frame is available
     if (flag_new_cnav_frame == true)
         {
-            std::cout << "NEW CNAV FRAME" << std::endl;
             std::bitset<GPS_L5_CNAV_DATA_PAGE_BITS> raw_bits;
             //Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder
             for (u32 i = 0; i < GPS_L5_CNAV_DATA_PAGE_BITS ; i++)
@@ -177,31 +169,31 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__((
                 {
                     // get ephemeris object for this SV
                     std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(d_CNAV_Message.get_ephemeris());
-                    std::cout << "New GPS CNAV message received: ephemeris from satellite " << d_satellite << std::endl;
+                    std::cout << "New GPS L5 CNAV message received: ephemeris from satellite " << d_satellite << std::endl;
                     this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
 
                 }
             if (d_CNAV_Message.have_new_iono() == true)
                 {
                     std::shared_ptr<Gps_CNAV_Iono> tmp_obj = std::make_shared<Gps_CNAV_Iono>(d_CNAV_Message.get_iono());
-                    std::cout << "New GPS CNAV message received: iono model parameters from satellite " << d_satellite << std::endl;
+                    std::cout << "New GPS L5 CNAV message received: iono model parameters from satellite " << d_satellite << std::endl;
                     this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
                 }
 
             if (d_CNAV_Message.have_new_utc_model() == true)
                 {
                     std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(d_CNAV_Message.get_utc_model());
-                    std::cout << "New GPS CNAV message received: UTC model parameters from satellite " << d_satellite << std::endl;
+                    std::cout << "New GPS L5 CNAV message received: UTC model parameters from satellite " << d_satellite << std::endl;
                     this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
                 }
 
             //update TOW at the preamble instant
-            d_TOW_at_Preamble = static_cast<int>(msg.tow);
+            d_TOW_at_Preamble = static_cast<double>(msg.tow) * 6.0;
             //* The time of the last input symbol can be computed from the message ToW and
             //* delay by the formulae:
             //* \code
-            //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20
+            //* symbolTime_ms = msg->tow * 6000 + *pdelay * 10
-            d_TOW_at_current_symbol = static_cast<double>(msg.tow) * 6.0 + static_cast<double>(delay) * GPS_L5i_PERIOD + 6 * GPS_L5i_PERIOD;
+            d_TOW_at_current_symbol = (static_cast<double>(msg.tow) * 6.0) + (static_cast<double>(delay) + 12.0) * GPS_L5i_SYMBOL_PERIOD;
             d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0;
             d_flag_valid_word = true;
         }
@@ -232,7 +224,7 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__((
             }
             catch (const std::ifstream::failure & e)
             {
-                    LOG(WARNING) << "Exception writing observables dump file " << e.what();
+                    LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what();
             }
         }
 
@@ -246,12 +238,14 @@ void gps_l5_telemetry_decoder_cc::set_satellite(const Gnss_Satellite & satellite
 {
     d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
     LOG(INFO) << "GPS L5 CNAV telemetry decoder in channel " << this->d_channel << " set to satellite " << d_satellite;
+    d_CNAV_Message.reset();
 }
 
 
 void gps_l5_telemetry_decoder_cc::set_channel(int channel)
 {
     d_channel = channel;
+    d_CNAV_Message.reset();
     LOG(INFO) << "GPS L5 CNAV channel set to " << channel;
     // ############# ENABLE DATA FILE LOG #################
     if (d_dump == true)
@@ -270,7 +264,7 @@ void gps_l5_telemetry_decoder_cc::set_channel(int channel)
                     }
                     catch (const std::ifstream::failure &e)
                     {
-                            LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
+                            LOG(WARNING) << "channel " << d_channel << " Exception opening Telemetry GPS L5 dump file " << e.what();
                     }
                 }
         }

src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h

@@ -58,7 +58,7 @@ gps_l5_telemetry_decoder_cc_sptr
 gps_l5_make_telemetry_decoder_cc(const Gnss_Satellite & satellite, bool dump);
 
 /*!
- * \brief This class implements a block that decodes the SBAS integrity and corrections data defined in RTCA MOPS DO-229
+ * \brief This class implements a GPS L5 Telemetry decoder
  *
  */
 class gps_l5_telemetry_decoder_cc : public gr::block
@@ -67,10 +67,6 @@ public:
     ~gps_l5_telemetry_decoder_cc();
     void set_satellite(const Gnss_Satellite & satellite);  //!< Set satellite PRN
     void set_channel(int channel);                         //!< Set receiver's channel
-
-    /*!
-     * \brief This is where all signal processing takes place
-     */
     int general_work (int noutput_items, gr_vector_int &ninput_items,
             gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
 
@@ -89,9 +85,6 @@ private:
 
     cnav_msg_decoder_t d_cnav_decoder;
 
-    int d_state;
-    int d_crc_error_count;
-
     double d_TOW_at_current_symbol;
     double d_TOW_at_Preamble;
     bool d_flag_valid_word;

src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c

@@ -411,7 +411,8 @@ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
  * The time of the last input symbol can be computed from the message ToW and
  * delay by the formulae:
  * \code
- * symbolTime_ms = msg->tow * 6000 + *pdelay * 20
+ * symbolTime_ms = msg->tow * 6000 + *pdelay * 20 (L2)
+ * symbolTime_ms = msg->tow * 6000 + *pdelay * 10 (L5)
  * \endcode
  *
  * \param[in,out] dec    Decoder object.

src/core/system_parameters/GPS_L5.h

@@ -53,6 +53,7 @@ const double GPS_L5_FREQ_HZ = FREQ5;             //!< L5 [Hz]
 const double GPS_L5i_CODE_RATE_HZ = 10.23e6;   //!< GPS L5i code rate [chips/s]
 const int GPS_L5i_CODE_LENGTH_CHIPS = 10230;   //!< GPS L5i  code length [chips]
 const double GPS_L5i_PERIOD = 0.001;           //!< GPS L5 code period [seconds]
+const double GPS_L5i_SYMBOL_PERIOD = 0.01;     //!< GPS L5 symbol period [seconds]
 
 const double GPS_L5q_CODE_RATE_HZ = 10.23e6;   //!< GPS L5i code rate [chips/s]
 const int GPS_L5q_CODE_LENGTH_CHIPS = 10230;    //!< GPS L5i code length [chips]

src/core/system_parameters/gps_cnav_navigation_message.cc

@@ -40,6 +40,7 @@ void Gps_CNAV_Navigation_Message::reset()
     b_flag_ephemeris_1 = false;
     b_flag_ephemeris_2 = false;
     b_flag_iono_valid = false;
+    b_flag_utc_valid = false;
 
     // satellite positions
     d_satpos_X = 0;