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

Added vision delay compensation to LPE. (#5585)

Browse Source
This commit is contained in:
James Goppert
2016-10-03 02:28:46 -04:00
committed by GitHub
parent 2f6abf26a3
commit 7c2798269c
5 changed files with 50 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/gps.cpp
+3 -17
View File
@@ -166,24 +166,10 @@ void BlockLocalPositionEstimator::gpsCorrect()
R(4, 4) = var_vxy;
R(5, 5) = var_vz;
// get delayed x and P
float t_delay = 0;
int i_hist = 0;
// get delayed x
uint8_t i_hist = 0;
for (i_hist = 1; i_hist < HIST_LEN; i_hist++) {
t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0));
if (t_delay > _gps_delay.get()) {
break;
}
}
// if you are 3 steps past the delay you wanted, this
// data is probably too old to use
if (t_delay > GPS_DELAY_MAX) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps delayed data too old: %8.4f", double(t_delay));
return;
}
if (getDelayPeriods(_gps_delay.get(), &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);