Showing
1 changed file
with
15 additions
and
3 deletions
... | @@ -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 | ... | ... |
-
Please register or login to post a comment