fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 08:53:34 +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/baro.cpp
+7 -7
View File
@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::baroInit()
// if finished
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltHome = _baroStats.getMean()(0);
_baroAltOrigin = _baroStats.getMean()(0);
mavlink_and_console_log_info(&mavlink_log_pub,
"[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0),
@@ -29,10 +29,10 @@ void BlockLocalPositionEstimator::baroInit()
_baroInitialized = true;
_baroFault = FAULT_NONE;
// only initialize alt home with baro if gps is disabled
if (!_altHomeInitialized && !_gps_on.get()) {
_altHomeInitialized = true;
_altHome = _baroAltHome;
// only initialize alt origin with baro and no gps
if (!_altOriginInitialized && !_gps_on.get()) {
_altOriginInitialized = true;
_altOrigin = _baroAltOrigin;
}
}
}
@@ -54,8 +54,8 @@ void BlockLocalPositionEstimator::baroCorrect()
if (baroMeasure(y) != OK) { return; }
// subtract baro home alt
y -= _baroAltHome;
// subtract baro origin alt
y -= _baroAltOrigin;
// baro measurement matrix
Matrix<float, n_y_baro, n_x> C;
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();
src/modules/local_position_estimator/sensors/mocap.cpp
+6 -6
View File
@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::mocapInit()
// if finished
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
_mocapHome = _mocapStats.getMean();
_mocapOrigin = _mocapStats.getMean();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
@@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::mocapInit()
_mocapInitialized = true;
_mocapFault = FAULT_NONE;
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _mocapHome(2);
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _mocapOrigin(2);
}
}
}
@@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
if (mocapMeasure(y) != OK) { return; }
// make measurement relative to home
y -= _mocapHome;
// make measurement relative to origin
y -= _mocapOrigin;
// mocap measurement matrix, measures position
Matrix<float, n_y_mocap, n_x> C;
src/modules/local_position_estimator/sensors/vision.cpp
+6 -6
View File
@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::visionInit()
// increament sums for mean
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
_visionHome = _visionStats.getMean();
_visionOrigin = _visionStats.getMean();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
"%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
double(_visionStats.getMean()(0)),
@@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::visionInit()
_visionInitialized = true;
_visionFault = FAULT_NONE;
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _visionHome(2);
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _visionOrigin(2);
}
}
}
@@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::visionCorrect()
if (visionMeasure(y) != OK) { return; }
// make measurement relative to home
y -= _visionHome;
// make measurement relative to origin
y -= _visionOrigin;
// vision measurement matrix, measures position
Matrix<float, n_y_vision, n_x> C;