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

INAV: clean up vision timestamps

This commit is contained in:
Lorenz Meier 2016-07-10 12:44:00 +02:00
parent 8b3045baa2
commit 7601788c43
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@ -769,8 +769,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
static hrt_abstime last_vision_time = 0;
float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
last_vision_time = vision.timestamp_boot;
float vision_dt = (vision.timestamp - last_vision_time) / 1e6f;
last_vision_time = vision.timestamp;
if (vision_dt > 0.000001f && vision_dt < 0.2f) {
vision.vx = (vision.x - last_vision_x) / vision_dt;
@ -951,14 +951,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* check for timeout on vision topic */
if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
if (vision_valid && (t > (vision.timestamp + vision_topic_timeout))) {
vision_valid = false;
warnx("VISION timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");
}
/* check for timeout on mocap topic */
if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
if (mocap_valid && (t > (mocap.timestamp + mocap_topic_timeout))) {
mocap_valid = false;
warnx("MOCAP timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");