Mara Branzanti GSoC commit September 2th.

Galileo navigation queues


git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@413 64b25241-fba3-4117-9849-534c7e92360d

src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc

@@ -318,7 +318,7 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items, gr_vector
 							if (d_nav.have_new_iono_and_GST()==true)
 							{
 								Galileo_Iono iono=d_nav.get_iono(); //notice that the read operation will clear the valid flag
-								std::cout<<"New Galileo IONO model received for SV "<<d_satellite.get_PRN()<<std::endl;
+								std::cout<<"New Galileo IONO model (and UTC) received for SV "<<d_satellite.get_PRN()<<std::endl;
 								d_iono_queue->push(iono);
 							}
 							if (d_nav.have_new_utc_model()==true)

src/core/receiver/control_thread.cc

@@ -161,7 +161,8 @@ void ControlThread::run()
     gps_acq_assist_data_collector_thread_=boost::thread(&ControlThread::gps_acq_assist_data_collector,this);
 
     galileo_ephemeris_data_collector_thread_ =boost::thread(&ControlThread::galileo_ephemeris_data_collector, this);
-
+    galileo_iono_data_collector_thread_ =boost::thread(&ControlThread::galileo_iono_data_collector, this);
+    galileo_utc_model_data_collector_thread_ =boost::thread(&ControlThread::galileo_utc_model_data_collector, this);
     // Main loop to read and process the control messages
     while (flowgraph_->running() && !stop_)
         {
@@ -180,7 +181,8 @@ void ControlThread::run()
 
     //Join Galileo threads
     galileo_ephemeris_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
-
+    galileo_iono_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
+    galileo_utc_model_data_collector_thread_.timed_join(boost::posix_time::seconds(1));
     //Join keyboard threads
     keyboard_thread_.timed_join(boost::posix_time::seconds(1));
 
@@ -479,6 +481,7 @@ void ControlThread::gps_ephemeris_data_collector()
 	}
 }
 
+
 void ControlThread::galileo_ephemeris_data_collector()
 {
 
@@ -492,31 +495,35 @@ void ControlThread::galileo_ephemeris_data_collector()
 			// DEBUG MESSAGE
 			std::cout << "Ephemeris record has arrived from SAT ID "
 					<< galileo_eph.SV_ID_PRN_4 << std::endl;
-     		// insert new ephemeris record to the global ephemeris map
+
+			// insert new ephemeris record to the global ephemeris map
 			if (global_galileo_ephemeris_map.read(galileo_eph.SV_ID_PRN_4,galileo_eph_old))
 			{
 				// Check the EPHEMERIS timestamp. If it is newer, then update the ephemeris
-				if (galileo_eph.WN_5 > galileo_eph_old.WN_5)
-				{
-					std::cout << "Ephemeris record updated -- GALILEO Week Number ="<<galileo_eph.WN_5<<std::endl;
-					global_galileo_ephemeris_map.write(galileo_eph.SV_ID_PRN_4,galileo_eph);
-				}else{
-					if (galileo_eph.TOW_5 > galileo_eph_old.TOW_5)
+					if (galileo_eph.WN_5 > galileo_eph_old.WN_5) //further check because it is not clear when IOD is reset
 					{
-						std::cout << "Ephemeris record updated -- GALILEO TOW ="<<galileo_eph.TOW_5<<std::endl;
+						std::cout << "Ephemeris record in global map updated -- GALILEO Week Number ="<<galileo_eph.WN_5<<std::endl;
 						global_galileo_ephemeris_map.write(galileo_eph.SV_ID_PRN_4,galileo_eph);
-
-						std::cout << "Ephemeris tow OLD: " << galileo_eph_old.TOW_5<<std::endl;
-						std::cout << "Ephemeris satellite: " << galileo_eph.SV_ID_PRN_4<<std::endl;
-
 					}else{
-						std::cout<<"Not updating the existing ephemeris"<<std::endl;
+						if (galileo_eph.IOD_ephemeris > galileo_eph_old.IOD_ephemeris)
+						{
+							std::cout << "Ephemeris record updated in global map-- IOD_ephemeris ="<<galileo_eph.IOD_ephemeris<<std::endl;
+							global_galileo_ephemeris_map.write(galileo_eph.SV_ID_PRN_4,galileo_eph);
+
+							std::cout << "IOD_ephemeris OLD: " << galileo_eph_old.IOD_ephemeris<<std::endl;
+							std::cout << "satellite: " << galileo_eph.SV_ID_PRN_4<<std::endl;
+						}
+						else{
+							std::cout<<"Not updating the existing ephemeris, IOD is not changing"<<std::endl;
+						}
 					}
-				}
+
 //
 			}else{
 			// insert new ephemeris record
-				std::cout << "New Ephemeris record inserted with TOW="<<galileo_eph.TOW_5<<" and GALILEO Week Number="<< galileo_eph.WN_5 << std::endl;
+				std::cout << "New Ephemeris record inserted in global map with TOW ="<<galileo_eph.TOW_5
+						  <<", GALILEO Week Number ="<< galileo_eph.WN_5
+						  << " and Ephemeris IOD = " << galileo_eph.IOD_ephemeris << std::endl;
 				global_galileo_ephemeris_map.write(galileo_eph.SV_ID_PRN_4, galileo_eph);
 			}
 	}
@@ -548,6 +555,49 @@ void ControlThread::gps_iono_data_collector()
 	}
 }
 
+void ControlThread::galileo_iono_data_collector()
+{
+
+	Galileo_Iono galileo_iono;
+	Galileo_Iono galileo_iono_old;
+	while(stop_==false)
+	{
+		global_galileo_iono_queue.wait_and_pop(galileo_iono);
+
+			// DEBUG MESSAGE
+			std::cout << "Iono record has arrived"<<std::endl;
+
+			// insert new Iono record to the global Iono map
+			if (global_galileo_iono_map.read(0,galileo_iono_old))
+     		{
+				// Check the Iono timestamp from UTC page (page 6). If it is newer, then update the Iono parameters
+					if (galileo_iono.WNot_6 > galileo_iono_old.WNot_6)
+					{
+						std::cout << "IONO record updated in global map--new GALILEO UTC-IONO Week Number"<< std::endl;
+						global_galileo_iono_map.write(0,galileo_iono);
+					}else{
+						if (galileo_iono.t0t_6 > galileo_iono_old.t0t_6)
+						{
+							std::cout << "IONO record updated in global map--new GALILEO UTC-IONO time of Week" << std::endl;
+							global_galileo_iono_map.write(0,galileo_iono);
+							//std::cout << "GALILEO IONO time of Week old: " << galileo_iono_old.t0t_6<<std::endl;
+
+						}
+						else{
+							std::cout<<"Not updating the existing Iono parameters in global map, Iono timestamp is not changing"<<std::endl;
+						}
+					}
+
+
+			}else{
+			// insert new ephemeris record
+				std::cout << "New IONO record inserted in global map" << std::endl;
+				global_galileo_iono_map.write(0, galileo_iono);
+			}
+	}
+}
+
+
 void ControlThread::gps_utc_model_data_collector()
 {
 
@@ -570,6 +620,46 @@ void ControlThread::gps_utc_model_data_collector()
 	}
 }
 
+void ControlThread::galileo_utc_model_data_collector()
+{
+	Galileo_Utc_Model galileo_utc;
+	Galileo_Utc_Model galileo_utc_old;
+	while(stop_==false)
+	{
+		global_galileo_utc_model_queue.wait_and_pop(galileo_utc);
+
+			// DEBUG MESSAGE
+			std::cout << "UTC record has arrived"<<std::endl;
+
+			// insert new UTC record to the global UTC map
+			if (global_galileo_utc_model_map.read(0,galileo_utc_old))
+			{
+				// Check the UTC timestamp. If it is newer, then update the ephemeris
+					if (galileo_utc.WNot_6 > galileo_utc_old.WNot_6) //further check because it is not clear when IOD is reset
+					{
+						//std::cout << "UTC record updated --new GALILEO UTC Week Number ="<<galileo_utc.WNot_6<<std::endl;
+						global_galileo_utc_model_map.write(0,galileo_utc);
+					}else{
+						if (galileo_utc.t0t_6 > galileo_utc_old.t0t_6)
+						{
+							//std::cout << "UTC record updated --new GALILEO UTC time of Week ="<<galileo_utc.t0t_6<<std::endl;
+							global_galileo_utc_model_map.write(0,galileo_utc);
+
+							//std::cout << "GALILEO UTC time of Week old: " << galileo_utc_old.t0t_6<<std::endl;
+
+						}
+						else{
+							std::cout<<"Not updating the existing UTC in global map, timestamp is not changing"<<std::endl;
+							}
+					}
+
+			}else{
+			// insert new ephemeris record
+				std::cout << "New UTC record inserted in global map" << std::endl;
+				global_galileo_utc_model_map.write(0, galileo_utc);
+			}
+	}
+}
 
 void ControlThread::gps_ephemeris_data_write_to_XML()
 {

src/core/receiver/control_thread.h

@@ -163,7 +163,12 @@ private:
      * \brief Blocking function that reads the Galileo ephemeris queue and updates the shared ephemeris map, accessible from the PVT block
      */
     void galileo_ephemeris_data_collector();
+    /*
+    * \brief Blocking function that reads the UTC model queue and updates the shared map, accessible from the PVT block
+    */
+    void galileo_utc_model_data_collector();
 
+    void galileo_iono_data_collector();
 
 
     void apply_action(unsigned int what);
@@ -183,7 +188,8 @@ private:
     boost::thread gps_acq_assist_data_collector_thread_;
 
     boost::thread galileo_ephemeris_data_collector_thread_;
-
+    boost::thread galileo_utc_model_data_collector_thread_;
+    boost::thread galileo_iono_data_collector_thread_;
     void keyboard_listener();
 };
 

src/core/system_parameters/galileo_ephemeris.cc

@@ -35,6 +35,10 @@
 
 Galileo_Ephemeris::Galileo_Ephemeris()
 {
+	flag_all_ephemeris = false;
+	IOD_ephemeris = 0;
+	IOD_nav_1 = 0;
+
 	 SV_ID_PRN_4 = 0;
 	 M0_1 = 0;		// Mean anomaly at reference time [semi-circles]
 	 delta_n_3 = 0;		// Mean motion difference from computed value  [semi-circles/sec]

src/core/system_parameters/galileo_ephemeris.h

@@ -51,6 +51,9 @@ public:
 
 	/*Galileo ephemeris are 16 parameters and here are reported following the ICD order, paragraph 5.1.1.
 	The number in the name after underscore (_1, _2, _3 and so on) refers to the page were we can find that parameter */
+	bool flag_all_ephemeris;
+	int IOD_ephemeris;
+	int IOD_nav_1;
 
 	int SV_ID_PRN_4;
 	double M0_1;		// Mean anomaly at reference time [semi-circles]

src/core/system_parameters/galileo_iono.cc

@@ -34,7 +34,22 @@
 
 Galileo_Iono::Galileo_Iono()
 {
-    valid = false;
+    //valid = false;
+	/*Ionospheric correction*/
+	/*Az*/
+	 ai0_5 = 0;	//Effective Ionisation Level 1st order parameter [sfu]
+	 ai1_5 = 0;		//Effective Ionisation Level 2st order parameter [sfu/degree]
+	 ai2_5 = 0;		//Effective Ionisation Level 3st order parameter [sfu/degree]
+
+	/*Ionospheric disturbance flag*/
+	 Region1_flag_5 = false;	// Ionospheric Disturbance Flag for region 1
+	 Region2_flag_5 = false;	// Ionospheric Disturbance Flag for region 2
+	 Region3_flag_5 = false;	// Ionospheric Disturbance Flag for region 3
+	 Region4_flag_5 = false;	// Ionospheric Disturbance Flag for region 4
+	 Region5_flag_5 = false;	// Ionospheric Disturbance Flag for region 5
+
+	 t0t_6 = 0;
+     WNot_6 = 0;
 
 }
 

src/core/system_parameters/galileo_iono.h

@@ -45,7 +45,7 @@ private:
 public:
 
 	// valid flag
-    bool valid;
+    //bool valid;
 
     /*Ionospheric correction*/
     /*Az*/
@@ -60,16 +60,12 @@ public:
     bool Region4_flag_5;	// Ionospheric Disturbance Flag for region 4
     bool Region5_flag_5;	// Ionospheric Disturbance Flag for region 5
 
-    // Ionospheric parameters GPS
-    /*double d_alpha0;      //!< Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s]
-    double d_alpha1;      //!< Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle]
-    double d_alpha2;      //!< Coefficient 2 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^2]
-    double d_alpha3;      //!< Coefficient 3 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^3]
-    double d_beta0;       //!< Coefficient 0 of a cubic equation representing the period of the model [s]
-    double d_beta1;       //!< Coefficient 1 of a cubic equation representing the period of the model [s/semi-circle]
-    double d_beta2;       //!< Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2]
-    double d_beta3;       //!< Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3]*/
-    /*!
+    /*from page 6 (UTC) to have a timestamp*/
+	double t0t_6;//UTC data reference Time of Week [s]
+	double WNot_6; //UTC data reference Week number [week]
+
+
+     /*!
      * Default constructor
      */
     Galileo_Iono();

src/core/system_parameters/galileo_navigation_message.cc

@@ -62,7 +62,7 @@ void Galileo_Navigation_Message::reset()
 	 flag_almanac_4 = false;      // flag indicating that almanac 4/4 (word 10) have been received
 
 
-
+	 IOD_ephemeris = 0;
 	 /*Word type 1: Ephemeris (1/4)*/
 	 IOD_nav_1 = 0;
 	 t0e_1 = 0;
@@ -444,11 +444,7 @@ void Galileo_Navigation_Message::split_page(const char *page, int flag_even_word
 						//std::cout<<"Data j k ephemeris" << endl << Data_jk_ephemeris << endl;
 
 						page_jk_decoder(Data_jk_ephemeris.c_str()); // Corresponding to ephemeris_decode.m in matlab code
-
-						/*if (have_new_ephemeris()==true){
-							std::cout<<"All ephemeris have been received" << std::endl;
-						}*/
-
+/*this block is just to try some function, it must be eliminated
 						double t_GST;
 						if ((have_new_iono_and_GST() == true) and (flag_all_ephemeris==true))
 						{
@@ -465,7 +461,7 @@ void Galileo_Navigation_Message::split_page(const char *page, int flag_even_word
 							t_UTC = GST_to_UTC_time(t_GST, WN_5);
 							std::cout << "UTC [sec]: " << t_UTC << std::endl;
 						}
-
+*/
 					}else{
 						// CRC wrong.. discard frame
 						std::cout<<"CRC error!"<<std::endl;
@@ -491,19 +487,23 @@ void Galileo_Navigation_Message::split_page(const char *page, int flag_even_word
 
 bool Galileo_Navigation_Message::have_new_ephemeris() //Check if we have a new ephemeris stored in the galileo navigation class
 {
-	/*std::cout << "flag ephememeris 1: " << flag_ephemeris_1 <<std::endl;
-	std::cout << "flag ephememeris 2: " << flag_ephemeris_2 <<std::endl;
-	std::cout << "flag ephememeris 3: " << flag_ephemeris_3 <<std::endl;
-	std::cout << "flag ephememeris 4: " << flag_ephemeris_4 <<std::endl;*/
 	if ((flag_ephemeris_1 == true) and (flag_ephemeris_2 == true) and (flag_ephemeris_3 == true) and (flag_ephemeris_4 == true))
 	{
-		std::cout<< "All ephemeris have been received"<< std::endl;
-		flag_ephemeris_1 = false;// clear the flag
-		flag_ephemeris_2 = false;// clear the flag
-		flag_ephemeris_3 = false;// clear the flag
-		flag_ephemeris_4 = false;// clear the flag
-		flag_all_ephemeris = true;
-		return true;
+		//if all ephemeris pages have the same IOD, then they belong to the same block
+			if ((IOD_nav_1 == IOD_nav_2) and (IOD_nav_3 == IOD_nav_4) and (IOD_nav_1 == IOD_nav_3))
+			{
+				std::cout<< "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch"<< std::endl;
+				flag_ephemeris_1 = false;// clear the flag
+				flag_ephemeris_2 = false;// clear the flag
+				flag_ephemeris_3 = false;// clear the flag
+				flag_ephemeris_4 = false;// clear the flag
+				flag_all_ephemeris = true;
+				IOD_ephemeris = IOD_nav_1;
+				std::cout << "Batch number: "<< IOD_ephemeris << std::endl;
+					return true;
+			}else{
+				return false;
+			}
 	 }
 	else
 		return false;
@@ -512,7 +512,7 @@ bool Galileo_Navigation_Message::have_new_ephemeris() //Check if we have a new e
 
 bool Galileo_Navigation_Message::have_new_iono_and_GST() //Check if we have a new iono data set stored in the galileo navigation class
 {
-	if (flag_iono_and_GST == true)
+	if ((flag_iono_and_GST == true) and (flag_utc_model == true)) //the condition on flag_utc_model is added to have a time stamp for iono
 	{
 		flag_iono_and_GST=false; // clear the flag
 		return true;
@@ -553,6 +553,8 @@ Galileo_Ephemeris Galileo_Navigation_Message::get_ephemeris()
 {
 
 	Galileo_Ephemeris ephemeris;
+	ephemeris.flag_all_ephemeris = flag_all_ephemeris;
+	ephemeris.IOD_ephemeris = IOD_ephemeris;
 	ephemeris.SV_ID_PRN_4 = SV_ID_PRN_4;
 	ephemeris.M0_1 = M0_1;		// Mean anomaly at reference time [semi-circles]
 	ephemeris.delta_n_3 = delta_n_3;		// Mean motion difference from computed value  [semi-circles/sec]
@@ -600,6 +602,8 @@ Galileo_Iono Galileo_Navigation_Message::get_iono()
 	 iono.Region4_flag_5 = Region4_flag_5;	// Ionospheric Disturbance Flag for region 4
 	 iono.Region5_flag_5 = Region5_flag_5;	// Ionospheric Disturbance Flag for region 5
 
+	 iono.t0t_6 = t0t_6;
+     iono.WNot_6 = WNot_6;
 	return iono;
 }
 
@@ -617,7 +621,7 @@ Galileo_Utc_Model Galileo_Navigation_Message::get_utc_model()
 	utc_model.WN_LSF_6 = WN_LSF_6;
 	utc_model.DN_6 = DN_6;
 	utc_model.Delta_tLSF_6 = Delta_tLSF_6;
-
+    utc_model.flag_utc_model = flag_utc_model;
 	/*GST*/
 	//utc_model.WN_5 = WN_5; //Week number
 	//utc_model.TOW_5 = WN_5; //Time of Week

src/core/system_parameters/galileo_navigation_message.h

@@ -88,6 +88,7 @@ public:
 	bool flag_almanac_3;      // flag indicating that almanac 3/4 (word 9) have been received
 	bool flag_almanac_4;      // flag indicating that almanac 4/4 (word 10) have been received
 
+	int IOD_ephemeris;
 	/*Word type 1: Ephemeris (1/4)*/
 		int IOD_nav_1;  // IOD_nav page 1
 		double t0e_1; 	// Ephemeris reference time [s]

src/core/system_parameters/galileo_utc_model.cc

@@ -42,7 +42,7 @@ Galileo_Utc_Model::Galileo_Utc_Model()
 	WN_LSF_6 = 0;
 	DN_6 = 0;
 	Delta_tLSF_6 = 0;
-
+	flag_utc_model = false;
 }
 
 double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int WN)

src/core/system_parameters/galileo_utc_model.h

@@ -51,29 +51,14 @@ public:
 	double A0_6;
 	double A1_6;
 	double Delta_tLS_6;
-	double t0t_6;
-	double WNot_6;
+	double t0t_6;//UTC data reference Time of Week [s]
+	double WNot_6; //UTC data reference Week number [week]
 	double WN_LSF_6;
 	double DN_6;
 	double Delta_tLSF_6;
+	bool flag_utc_model;
 	//double TOW_6;
 
-	/*GST*/
-	//double WN_5; //Week number
-	//double TOW_5; //Time of Week
-
-	//this is from gps
-	/*bool valid;
-    // UTC parameters
-    double d_A1;          //!< 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200E) [s/s]
-    double d_A0;          //!< Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200E) [s]
-    double d_t_OT;        //!< Reference time for UTC data (reference 20.3.4.5 and 20.3.3.5.2.4 IS-GPS-200E) [s]
-    int i_WN_T;           //!< UTC reference week number [weeks]
-    double d_DeltaT_LS;   //!< delta time due to leap seconds [s]. Number of leap seconds since 6-Jan-1980 as transmitted by the GPS almanac.
-    int i_WN_LSF;         //!< Week number at the end of which the leap second becomes effective [weeks]
-    int i_DN;             //!< Day number (DN) at the end of which the leap second becomes effective [days]
-    double d_DeltaT_LSF;  //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
-*/
 
 
 	double GST_to_UTC_time(double t_e, int WN);