이충섭

feat : print current gps every 2 seconds

...@@ -40,7 +40,7 @@ ...@@ -40,7 +40,7 @@
40 */ 40 */
41 41
42 #include "ekf.h" 42 #include "ekf.h"
43 - 43 +#include <iostream>
44 #include <ecl.h> 44 #include <ecl.h>
45 #include <geo_lookup/geo_mag_declination.h> 45 #include <geo_lookup/geo_mag_declination.h>
46 #include <mathlib/mathlib.h> 46 #include <mathlib/mathlib.h>
...@@ -55,12 +55,11 @@ ...@@ -55,12 +55,11 @@
55 #define MASK_GPS_VDRIFT (1<<6) 55 #define MASK_GPS_VDRIFT (1<<6)
56 #define MASK_GPS_HSPD (1<<7) 56 #define MASK_GPS_HSPD (1<<7)
57 #define MASK_GPS_VSPD (1<<8) 57 #define MASK_GPS_VSPD (1<<8)
58 - 58 +static int hold=0;
59 bool Ekf::collect_gps(const gps_message &gps) 59 bool Ekf::collect_gps(const gps_message &gps)
60 { 60 {
61 // Run GPS checks always 61 // Run GPS checks always
62 _gps_checks_passed = gps_is_good(gps); 62 _gps_checks_passed = gps_is_good(gps);
63 -
64 if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) { 63 if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
65 // If we have good GPS data set the origin's WGS-84 position to the last gps fix 64 // If we have good GPS data set the origin's WGS-84 position to the last gps fix
66 const double lat = gps.lat * 1.0e-7; 65 const double lat = gps.lat * 1.0e-7;
...@@ -142,6 +141,19 @@ bool Ekf::collect_gps(const gps_message &gps) ...@@ -142,6 +141,19 @@ bool Ekf::collect_gps(const gps_message &gps)
142 } 141 }
143 142
144 // start collecting GPS if there is a 3D fix and the NED origin has been set 143 // start collecting GPS if there is a 3D fix and the NED origin has been set
144 + if(hold != int(gps.time_usec/1000000))
145 + {
146 + hold=int(gps.time_usec/1000000);
147 + if(hold%2==0)
148 + {
149 + std::cout <<"----------------------current--------------------------"<<std::endl;
150 + std :: cout << "time : "<<gps.time_usec/1000000<<std::endl;
151 + std :: cout << "lat : " <<gps.lat <<std::endl;
152 + std :: cout << "lon : " <<gps.lon <<std::endl;
153 + std :: cout << "alt : " <<gps.alt <<std::endl;
154 + }
155 + }
156 + // print GPS data every 1 seocnds
145 return _NED_origin_initialised && (gps.fix_type >= 3); 157 return _NED_origin_initialised && (gps.fix_type >= 3);
146 } 158 }
147 159
......