feat : print current gps every 2 seconds

@@ -40,7 +40,7 @@
  */
 
 #include "ekf.h"
-
+#include <iostream>
 #include <ecl.h>
 #include <geo_lookup/geo_mag_declination.h>
 #include <mathlib/mathlib.h>
@@ -55,12 +55,11 @@
 #define MASK_GPS_VDRIFT (1<<6)
 #define MASK_GPS_HSPD   (1<<7)
 #define MASK_GPS_VSPD   (1<<8)
-
+static int hold=0;
 bool Ekf::collect_gps(const gps_message &gps)
 {
 	// Run GPS checks always
 	_gps_checks_passed = gps_is_good(gps);
-
 	if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
 		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
 		const double lat = gps.lat * 1.0e-7;
@@ -142,6 +141,19 @@ bool Ekf::collect_gps(const gps_message &gps)
 	}
 
 	// start collecting GPS if there is a 3D fix and the NED origin has been set
+	if(hold != int(gps.time_usec/1000000))
+	{
+	hold=int(gps.time_usec/1000000);
+		if(hold%2==0)
+		{
+			std::cout <<"----------------------current--------------------------"<<std::endl;
+			std :: cout << "time : "<<gps.time_usec/1000000<<std::endl;
+			std :: cout << "lat : " <<gps.lat <<std::endl;
+			std :: cout << "lon : " <<gps.lon <<std::endl;
+			std :: cout << "alt : " <<gps.alt <<std::endl;
+		}
+	}
+	// print GPS data every 1 seocnds
 	return _NED_origin_initialised && (gps.fix_type >= 3);
 }