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

lpe : use vision timestamps to compute measurement delay

Browse Source
This commit is contained in:
Kabir Mohammed 2017-01-16 12:20:54 +05:30 committed by Lorenz Meier
parent 85df00d2da
commit 0f91378eeb
2 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/local_position_estimator/params.c
View File

@ -219,7 +219,9 @@ PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f);
/**
* Vision delay compensaton
* Vision delay compensaton.
*
* Set to zero to enable automatic compensation from measurement timestamps
*
* @group Local Position Estimator
* @unit sec

10
src/modules/local_position_estimator/sensors/vision.cpp
View File

@ -73,6 +73,7 @@ void BlockLocalPositionEstimator::visionCorrect()
Matrix<float, n_y_vision, n_y_vision> R;
R.setZero();
// use error estimates from vision topic if available
if (_sub_vision_pos.get().eph > _vision_xy_stddev.get()) {
R(Y_vision_x, Y_vision_x) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
R(Y_vision_y, Y_vision_y) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
@ -92,7 +93,14 @@ void BlockLocalPositionEstimator::visionCorrect()
// vision delayed x
uint8_t i_hist = 0;
if (getDelayPeriods(_vision_delay.get(), &i_hist) < 0) { return; }
float vision_delay = (_timeStamp - _sub_vision_pos.get().timestamp) * 1e-6f; // measurement delay in seconds
if (vision_delay < 0.0f) { vision_delay = 0.0f; }
// use auto-calculated delay from measurement if parameter is set to zero
if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0) { return; }
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision delay : %0.2f ms", double(vision_delay * 1000.0f));
Vector<float, n_x> x0 = _xDelay.get(i_hist);