fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 20:20:34 +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/BlockLocalPositionEstimator.cpp
+24
View File
@@ -77,6 +77,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_gps_epv_max(this, "EPV_MAX"),
_vision_xy_stddev(this, "VIS_XY"),
_vision_z_stddev(this, "VIS_Z"),
_vision_delay(this, "VIS_DELAY"),
_vision_on(this, "VIS_ON"),
_mocap_p_stddev(this, "VIC_P"),
_flow_gyro_comp(this, "FLW_GYRO_CMP"),
@@ -913,3 +914,26 @@ void BlockLocalPositionEstimator::predict()
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
{
float t_delay = 0;
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 > delay) {
break;
}
}
*periods = i_hist;
if (t_delay > DELAY_MAX) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] delayed data too old: %8.4f", double(t_delay));
return -1;
}
return OK;
}