dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

LPE alt init now allows baro only init without GPS w/o changing flag. (#5398) 

Flight test went well, merging.
This commit is contained in:
James Goppert 2016-08-27 21:57:32 -04:00 committed by GitHub
parent 4499006744
commit be1417f613
2 changed files with 32 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/local_position_estimator/sensors/baro.cpp
View File

@ -29,8 +29,7 @@ void BlockLocalPositionEstimator::baroInit()
_baroInitialized = true; _baroInitialized = true;
_baroFault = FAULT_NONE; _baroFault = FAULT_NONE;
// only initialize alt origin with baro and no gps if (!_altOriginInitialized) {
if (!_altOriginInitialized && !_gps_on.get()) {
_altOriginInitialized = true; _altOriginInitialized = true;
_altOrigin = _baroAltOrigin; _altOrigin = _baroAltOrigin;
} }

46
src/modules/local_position_estimator/sensors/gps.cpp
View File

@ -37,27 +37,43 @@ void BlockLocalPositionEstimator::gpsInit()
// if finished // if finished
if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) { if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
double gpsLatOrigin = _gpsStats.getMean()(0); // get mean gps values
double gpsLonOrigin = _gpsStats.getMean()(1); 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; _gpsInitialized = true;
_gpsFault = FAULT_NONE; _gpsFault = FAULT_NONE;
_gpsStats.reset(); _gpsStats.reset();
if (!_altOriginInitialized) { if (!_receivedGps) {
_altOriginInitialized = true; // 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; _altOrigin = _gpsAltOrigin;
_altOriginInitialized = true;
PX4_INFO("[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
gpsLatOrigin,
gpsLonOrigin,
double(_gpsAltOrigin));
} }
} }
} }