From be1417f613592595ea0db4c7eac3409340b7909e Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Sat, 27 Aug 2016 21:57:32 -0400
Subject: [PATCH] LPE alt init now allows baro only init without GPS w/o
changing flag. (#5398)
Flight test went well, merging.
---
.../local_position_estimator/sensors/baro.cpp | 3 +-
.../local_position_estimator/sensors/gps.cpp | 46 +++++++++++++------
2 files changed, 32 insertions(+), 17 deletions(-)
diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index c41bfed60c..7bf908558d 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -29,8 +29,7 @@ void BlockLocalPositionEstimator::baroInit()
_baroInitialized = true;
_baroFault = FAULT_NONE;
- // only initialize alt origin with baro and no gps
- if (!_altOriginInitialized && !_gps_on.get()) {
+ if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _baroAltOrigin;
}
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index ff331072be..740ee0b3c2 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -37,27 +37,43 @@ void BlockLocalPositionEstimator::gpsInit()
// if finished
if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
- double gpsLatOrigin = _gpsStats.getMean()(0);
- double gpsLonOrigin = _gpsStats.getMean()(1);
+ // get mean gps values
+ double gpsLat = _gpsStats.getMean()(0);
+ double gpsLon = _gpsStats.getMean()(1);
+ float gpsAlt = _gpsStats.getMean()(2);
- if (!_receivedGps) {
- _receivedGps = true;
- map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
- }
-
- _gpsAltOrigin = _gpsStats.getMean()(2);
- PX4_INFO("[lpe] gps init "
- "lat %6.2f lon %6.2f alt %5.1f m",
- gpsLatOrigin,
- gpsLonOrigin,
- double(_gpsAltOrigin));
_gpsInitialized = true;
_gpsFault = FAULT_NONE;
_gpsStats.reset();
- if (!_altOriginInitialized) {
- _altOriginInitialized = true;
+ if (!_receivedGps) {
+ // this is the first time we have received gps
+ _receivedGps = true;
+
+ // note we subtract X_z which is in down directon so it is
+ // an addition
+ _gpsAltOrigin = gpsAlt + _x(X_z);
+
+ // find lat, lon of current origin by subtracting x and y
+ double gpsLatOrigin = 0;
+ double gpsLonOrigin = 0;
+ // reproject at current coordinates
+ map_projection_init(&_map_ref, gpsLat, gpsLon);
+ // find origin
+ map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
+ // reinit origin
+ map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
+
+ // always override alt origin on first GPS to fix
+ // possible baro offset in global altitude at init
_altOrigin = _gpsAltOrigin;
+ _altOriginInitialized = true;
+
+ PX4_INFO("[lpe] gps init "
+ "lat %6.2f lon %6.2f alt %5.1f m",
+ gpsLatOrigin,
+ gpsLonOrigin,
+ double(_gpsAltOrigin));
}
}
}
--
GitLab