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

vehicle_odometry: add timestamp_sample field for latency monitoring

Browse Source
This commit is contained in:
Mohammed Kabir
2020-04-28 13:58:43 -04:00
committed by Daniel Agar
parent ccaa103164
commit 5ffe88672e
9 changed files with 26 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+2 -1
View File
@@ -636,7 +636,8 @@ void BlockLocalPositionEstimator::publishOdom()
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
_pub_odom.get().timestamp = _timeStamp;
_pub_odom.get().timestamp = hrt_absolute_time();
_pub_odom.get().timestamp_sample = _timeStamp;
_pub_odom.get().local_frame = _pub_odom.get().LOCAL_FRAME_NED;
// position
src/modules/local_position_estimator/sensors/mocap.cpp
+2 -2
View File
@@ -79,11 +79,11 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
}
if (!_mocap_xy_valid || !_mocap_z_valid) {
_time_last_mocap = _sub_mocap_odom.get().timestamp;
_time_last_mocap = _sub_mocap_odom.get().timestamp_sample;
return -1;
} else {
_time_last_mocap = _sub_mocap_odom.get().timestamp;
_time_last_mocap = _sub_mocap_odom.get().timestamp_sample;
if (PX4_ISFINITE(_sub_mocap_odom.get().x)) {
y.setZero();
src/modules/local_position_estimator/sensors/vision.cpp
+3 -3
View File
@@ -84,11 +84,11 @@ int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
}
if (!_vision_xy_valid || !_vision_z_valid) {
_time_last_vision_p = _sub_visual_odom.get().timestamp;
_time_last_vision_p = _sub_visual_odom.get().timestamp_sample;
return -1;
} else {
_time_last_vision_p = _sub_visual_odom.get().timestamp;
_time_last_vision_p = _sub_visual_odom.get().timestamp_sample;
if (PX4_ISFINITE(_sub_visual_odom.get().x)) {
y.setZero();
@@ -147,7 +147,7 @@ void BlockLocalPositionEstimator::visionCorrect()
// vision delayed x
uint8_t i_hist = 0;
float vision_delay = (_timeStamp - _sub_visual_odom.get().timestamp) * 1e-6f; // measurement delay in seconds
float vision_delay = (_timeStamp - _sub_visual_odom.get().timestamp_sample) * 1e-6f; // measurement delay in seconds
if (vision_delay < 0.0f) { vision_delay = 0.0f; }