Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

vehicle_odometry: add timestamp_sample field for latency monitoring #14781

Merged
merged 1 commit into from
Apr 28, 2020
Merged

vehicle_odometry: add timestamp_sample field for latency monitoring #14781

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions msg/vehicle_odometry.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
# Vehicle odometry data. Fits ROS REP 147 for aerial vehicles
uint64 timestamp # time since system start (microseconds)

uint64 timestamp_sample

# Covariance matrix index constants
uint8 COVARIANCE_MATRIX_X_VARIANCE=0
uint8 COVARIANCE_MATRIX_Y_VARIANCE=6
Expand Down
4 changes: 2 additions & 2 deletions src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -274,7 +274,7 @@ AttitudeEstimatorQ::Run()
// vision external heading usage (ATT_EXT_HDG_M 1)
if (_param_att_ext_hdg_m.get() == 1) {
// Check for timeouts on data
_ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);
}
}
}
Expand Down Expand Up @@ -303,7 +303,7 @@ AttitudeEstimatorQ::Run()
// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
if (_param_att_ext_hdg_m.get() == 2) {
// Check for timeouts on data
_ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
}
}
}
Expand Down
6 changes: 4 additions & 2 deletions src/modules/ekf2/ekf2_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1169,7 +1169,7 @@ void Ekf2::Run()
}

// use timestamp from external computer, clocks are synchronized when using MAVROS
ev_data.time_us = _ev_odom.timestamp;
ev_data.time_us = _ev_odom.timestamp_sample;
_ekf.setExtVisionData(ev_data);

ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)_ev_odom.timestamp / 100 -
Expand Down Expand Up @@ -1230,7 +1230,9 @@ void Ekf2::Run()
vehicle_odometry_s odom{};

lpos.timestamp = now;
odom.timestamp = lpos.timestamp;

odom.timestamp = hrt_absolute_time();
odom.timestamp_sample = now;

odom.local_frame = odom.LOCAL_FRAME_NED;

Expand Down
3 changes: 2 additions & 1 deletion src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions src/modules/local_position_estimator/sensors/mocap.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
6 changes: 3 additions & 3 deletions src/modules/local_position_estimator/sensors/vision.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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; }

Expand Down
4 changes: 2 additions & 2 deletions src/modules/mavlink/mavlink_messages.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2558,7 +2558,7 @@ class MavlinkStreamOdometry : public MavlinkStream
}

if (odom_updated) {
msg.time_usec = odom.timestamp;
msg.time_usec = odom.timestamp_sample;
msg.child_frame_id = MAV_FRAME_BODY_FRD;

// Current position
Expand Down Expand Up @@ -2972,7 +2972,7 @@ class MavlinkStreamAttPosMocap : public MavlinkStream
if (_mocap_sub.update(&mocap)) {
mavlink_att_pos_mocap_t msg{};

msg.time_usec = mocap.timestamp;
msg.time_usec = mocap.timestamp_sample;
msg.q[0] = mocap.q[0];
msg.q[1] = mocap.q[1];
msg.q[2] = mocap.q[2];
Expand Down
11 changes: 8 additions & 3 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -773,7 +773,9 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)

vehicle_odometry_s mocap_odom{};

mocap_odom.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec);
mocap_odom.timestamp = hrt_absolute_time();
mocap_odom.timestamp_sample = _mavlink_timesync.sync_stamp(mocap.time_usec);

mocap_odom.x = mocap.x;
mocap_odom.y = mocap.y;
mocap_odom.z = mocap.z;
Expand Down Expand Up @@ -1251,7 +1253,9 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)

vehicle_odometry_s visual_odom{};

visual_odom.timestamp = _mavlink_timesync.sync_stamp(ev.usec);
visual_odom.timestamp = hrt_absolute_time();
visual_odom.timestamp_sample = _mavlink_timesync.sync_stamp(ev.usec);

visual_odom.x = ev.x;
visual_odom.y = ev.y;
visual_odom.z = ev.z;
Expand Down Expand Up @@ -1291,7 +1295,8 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)

vehicle_odometry_s odometry{};

odometry.timestamp = _mavlink_timesync.sync_stamp(odom.time_usec);
odometry.timestamp = hrt_absolute_time();
odometry.timestamp_sample = _mavlink_timesync.sync_stamp(odom.time_usec);

/* The position is in a local FRD frame */
odometry.x = odom.x;
Expand Down
1 change: 1 addition & 0 deletions src/modules/simulator/simulator_mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1011,6 +1011,7 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
struct vehicle_odometry_s odom;

odom.timestamp = timestamp;
odom.timestamp_sample = timestamp;

const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);

Expand Down