fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 05:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

LPE: Don't use home as local origin (#5067)

Browse Source
This commit is contained in:
James Goppert
2016-07-17 10:16:17 -04:00
committed by GitHub
parent 981353f439
commit 9fcf121380
8 changed files with 65 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/gps.cpp
+11 -11
View File
@@ -37,27 +37,27 @@ void BlockLocalPositionEstimator::gpsInit()
// if finished
if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
double gpsLatHome = _gpsStats.getMean()(0);
double gpsLonHome = _gpsStats.getMean()(1);
double gpsLatOrigin = _gpsStats.getMean()(0);
double gpsLonOrigin = _gpsStats.getMean()(1);
if (!_receivedGps) {
_receivedGps = true;
map_projection_init(&_map_ref, gpsLatHome, gpsLonHome);
map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
}
_gpsAltHome = _gpsStats.getMean()(2);
_gpsAltOrigin = _gpsStats.getMean()(2);
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
gpsLatHome,
gpsLonHome,
double(_gpsAltHome));
gpsLatOrigin,
gpsLonOrigin,
double(_gpsAltOrigin));
_gpsInitialized = true;
_gpsFault = FAULT_NONE;
_gpsStats.reset();
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _gpsAltHome;
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _gpsAltOrigin;
}
}
}
@@ -92,7 +92,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
float alt = y_global(2);
float px = 0;
float py = 0;
float pz = -(alt - _gpsAltHome);
float pz = -(alt - _gpsAltOrigin);
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector<float, 6> y;
y.setZero();