Toggle navigation
Toggle navigation
This project
Loading...
Sign in
2021-1-capstone-design1
/
JJS_Project2
Go to a project
Toggle navigation
Toggle navigation pinning
Projects
Groups
Snippets
Help
Project
Activity
Repository
Pipelines
Graphs
Issues
0
Merge Requests
0
Wiki
Snippets
Network
Create a new issue
Builds
Commits
Issue Boards
Authored by
이충섭
2021-06-05 16:27:27 +0900
Browse Files
Options
Browse Files
Download
Email Patches
Plain Diff
Commit
3a8b2bcd8ba4fdbc371437f3161e7fe7740a97cb
3a8b2bcd
1 parent
03de69f9
feat : print current gps every 2 seconds
Show whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
15 additions
and
3 deletions
home/Firmware/src/lib/ecl/EKF/gps_checks.cpp
home/Firmware/src/lib/ecl/EKF/gps_checks.cpp
View file @
3a8b2bc
...
...
@@ -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
);
}
...
...
Please
register
or
login
to post a comment