From 20c2fe6d285867b51ec68a5199391385cf524e1e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 1 Sep 2020 14:38:41 -0400 Subject: [PATCH] estimator messages add explicit timestamp_sample - timestamp is uORB publication metadata - this allows us to see what the system saw at publication time plus the latency in estimation --- msg/estimator_innovations.msg | 1 + msg/estimator_sensor_bias.msg | 1 + msg/estimator_states.msg | 1 + msg/estimator_status.msg | 1 + msg/vehicle_attitude.msg | 2 + msg/vehicle_global_position.msg | 1 + msg/vehicle_local_position.msg | 1 + msg/wind_estimate.msg | 2 + msg/yaw_estimator_status.msg | 5 +- .../attitude_estimator_q_main.cpp | 3 +- src/modules/ekf2/EKF2.cpp | 48 +++++++++++-------- .../BlockLocalPositionEstimator.cpp | 26 +++++++--- 12 files changed, 63 insertions(+), 29 deletions(-) diff --git a/msg/estimator_innovations.msg b/msg/estimator_innovations.msg index a49ab9c5c4..311e840e54 100644 --- a/msg/estimator_innovations.msg +++ b/msg/estimator_innovations.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) # GPS float32[2] gps_hvel # horizontal GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2) diff --git a/msg/estimator_sensor_bias.msg b/msg/estimator_sensor_bias.msg index 2a8debf0f0..e0884b158c 100644 --- a/msg/estimator_sensor_bias.msg +++ b/msg/estimator_sensor_bias.msg @@ -4,6 +4,7 @@ # uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) # In-run bias estimates (subtract from uncorrected data) diff --git a/msg/estimator_states.msg b/msg/estimator_states.msg index d66fd39c4c..f4e3f0f261 100644 --- a/msg/estimator_states.msg +++ b/msg/estimator_states.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) float32[24] states # Internal filter states uint8 n_states # Number of states effectively used diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index f8897da16e..209ff5bdb1 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) float32[3] vibe # IMU vibration metrics in the following array locations # 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 874e73804f..8fbc874374 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -2,6 +2,8 @@ uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + float32[4] q # Quaternion rotation from XYZ body frame to NED earth frame. float32[4] delta_q_reset # Amount by which quaternion has changed during last reset uint8 quat_reset_counter # Quaternion reset counter diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index cbbc1c60fd..3da984de66 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -6,6 +6,7 @@ # uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) float64 lat # Latitude, (degrees) float64 lon # Longitude, (degrees) diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 754bcf69ee..bc393b5f62 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -1,6 +1,7 @@ # Fused local position in NED. uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) bool xy_valid # true if x and y are valid bool z_valid # true if z is valid diff --git a/msg/wind_estimate.msg b/msg/wind_estimate.msg index 636ca70592..f6894d914c 100644 --- a/msg/wind_estimate.msg +++ b/msg/wind_estimate.msg @@ -1,4 +1,6 @@ uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + float32 windspeed_north # Wind component in north / X direction (m/sec) float32 windspeed_east # Wind component in east / Y direction (m/sec) diff --git a/msg/yaw_estimator_status.msg b/msg/yaw_estimator_status.msg index 39e8b041d5..4f6ab58956 100644 --- a/msg/yaw_estimator_status.msg +++ b/msg/yaw_estimator_status.msg @@ -1,6 +1,9 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + float32 yaw_composite # composite yaw from GSF (rad) float32 yaw_variance # composite yaw variance from GSF (rad^2) + float32[5] yaw # yaw estimate for each model in the filter bank (rad) float32[5] innov_vn # North velocity innovation for each model in the filter bank (m/s) float32[5] innov_ve # East velocity innovation for each model in the filter bank (m/s) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 139b975290..c4153f9dff 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -358,10 +358,11 @@ AttitudeEstimatorQ::Run() if (update(dt)) { vehicle_attitude_s att = {}; - att.timestamp = sensors.timestamp; + att.timestamp_sample = sensors.timestamp; _q.copyTo(att.q); /* the instance count is not used here */ + att.timestamp = hrt_absolute_time(); _att_pub.publish(att); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 14eacff510..be3419b952 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -743,14 +743,11 @@ void EKF2::Run() if (control_status.flags.tilt_align) { // generate vehicle local position data vehicle_local_position_s &lpos = _vehicle_local_position_pub.get(); + lpos.timestamp_sample = imu_sample_new.time_us; // generate vehicle odometry data vehicle_odometry_s odom{}; - - lpos.timestamp = now; - - odom.timestamp = hrt_absolute_time(); - odom.timestamp_sample = now; + odom.timestamp_sample = imu_sample_new.time_us; odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; @@ -922,9 +919,11 @@ void EKF2::Run() odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6]; // publish vehicle local position data + lpos.timestamp = _replay_mode ? now : hrt_absolute_time(); _vehicle_local_position_pub.update(); // publish vehicle odometry data + odom.timestamp = _replay_mode ? now : hrt_absolute_time(); _vehicle_odometry_pub.publish(odom); // publish external visual odometry after fixed frame alignment if new odometry is received @@ -975,8 +974,7 @@ void EKF2::Run() if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) { // generate and publish global position data vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get(); - - global_pos.timestamp = now; + global_pos.timestamp_sample = imu_sample_new.time_us; if (fabsf(lpos_x_prev - lpos.x) > FLT_EPSILON || fabsf(lpos_y_prev - lpos.y) > FLT_EPSILON) { map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &global_pos.lat, &global_pos.lon); @@ -1002,13 +1000,14 @@ void EKF2::Run() } global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning - + global_pos.timestamp = _replay_mode ? now : hrt_absolute_time(); _vehicle_global_position_pub.update(); } } // publish estimator states estimator_states_s states; + states.timestamp_sample = imu_sample_new.time_us; states.n_states = 24; _ekf.getStateAtFusionHorizonAsVector().copyTo(states.states); _ekf.covariances_diagonal().copyTo(states.covariances); @@ -1017,6 +1016,7 @@ void EKF2::Run() // publish estimator status estimator_status_s status{}; + status.timestamp_sample = imu_sample_new.time_us; _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); _ekf.get_gps_check_status(&status.gps_check_fail_flags); // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include @@ -1039,12 +1039,12 @@ void EKF2::Run() status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed; - + status.timestamp = _replay_mode ? now : hrt_absolute_time(); _estimator_status_pub.publish(status); { // publish all corrected sensor readings and bias estimates after mag calibration is updated above - bias.timestamp = now; + bias.timestamp_sample = imu_sample_new.time_us; // take device ids from sensor_selection_s if not using specific vehicle_imu_s if (_imu_sub_index < 0) { @@ -1062,6 +1062,7 @@ void EKF2::Run() bias.mag_bias[1] = _last_valid_mag_cal[1]; bias.mag_bias[2] = _last_valid_mag_cal[2]; + bias.timestamp = _replay_mode ? now : hrt_absolute_time(); _estimator_sensor_bias_pub.publish(bias); } @@ -1071,11 +1072,11 @@ void EKF2::Run() if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) { ekf_gps_drift_s drift_data; - drift_data.timestamp = now; drift_data.hpos_drift_rate = gps_drift[0]; drift_data.vpos_drift_rate = gps_drift[1]; drift_data.hspd = gps_drift[2]; drift_data.blocked = blocked; + drift_data.timestamp = _replay_mode ? now : hrt_absolute_time(); _ekf_gps_drift_pub.publish(drift_data); } @@ -1163,7 +1164,7 @@ void EKF2::Run() { // publish estimator innovation data estimator_innovations_s innovations; - innovations.timestamp = now; + innovations.timestamp_sample = imu_sample_new.time_us; _ekf.getGpsVelPosInnov(&innovations.gps_hvel[0], innovations.gps_vvel, &innovations.gps_hpos[0], innovations.gps_vpos); _ekf.getEvVelPosInnov(&innovations.ev_hvel[0], innovations.ev_vvel, &innovations.ev_hpos[0], innovations.ev_vpos); @@ -1184,7 +1185,7 @@ void EKF2::Run() // publish estimator innovation variance data estimator_innovations_s innovation_var; - innovation_var.timestamp = now; + innovation_var.timestamp_sample = imu_sample_new.time_us; _ekf.getGpsVelPosInnovVar(&innovation_var.gps_hvel[0], innovation_var.gps_vvel, &innovation_var.gps_hpos[0], innovation_var.gps_vpos); _ekf.getEvVelPosInnovVar(&innovation_var.ev_hvel[0], innovation_var.ev_vvel, &innovation_var.ev_hpos[0], @@ -1207,7 +1208,7 @@ void EKF2::Run() // publish estimator innovation test ratio data estimator_innovations_s test_ratios; - test_ratios.timestamp = now; + test_ratios.timestamp_sample = imu_sample_new.time_us; _ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0], test_ratios.gps_vpos); _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], @@ -1236,10 +1237,14 @@ void EKF2::Run() resetPreFlightChecks(); } + innovations.timestamp = _replay_mode ? now : hrt_absolute_time(); _estimator_innovations_pub.publish(innovations); + + innovation_var.timestamp = _replay_mode ? now : hrt_absolute_time(); _estimator_innovation_variances_pub.publish(innovation_var); - _estimator_innovation_test_ratios_pub.publish(test_ratios); + test_ratios.timestamp = _replay_mode ? now : hrt_absolute_time(); + _estimator_innovation_test_ratios_pub.publish(test_ratios); } } @@ -1318,13 +1323,12 @@ void EKF2::publish_attitude(const hrt_abstime ×tamp) if (_ekf.attitude_valid()) { // generate vehicle attitude quaternion data vehicle_attitude_s att; - att.timestamp = timestamp; - + att.timestamp_sample = timestamp; const Quatf q{_ekf.calculate_quaternion()}; q.copyTo(att.q); _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); - + att.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _att_pub.publish(att); } else if (_replay_mode) { @@ -1347,7 +1351,8 @@ void EKF2::publish_yaw_estimator_status(const hrt_abstime ×tamp) &yaw_est_test_data.innov_vn[0], &yaw_est_test_data.innov_ve[0], &yaw_est_test_data.weight[0])) { - yaw_est_test_data.timestamp = timestamp; + yaw_est_test_data.timestamp_sample = timestamp; + yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _yaw_est_pub.publish(yaw_est_test_data); } @@ -1358,18 +1363,21 @@ void EKF2::publish_wind_estimate(const hrt_abstime ×tamp) if (_ekf.get_wind_status()) { // Publish wind estimate only if ekf declares them valid wind_estimate_s wind_estimate{}; + wind_estimate.timestamp_sample = timestamp; + const Vector2f wind_vel = _ekf.getWindVelocity(); const Vector2f wind_vel_var = _ekf.getWindVelocityVariance(); _ekf.getAirspeedInnov(wind_estimate.tas_innov); _ekf.getAirspeedInnovVar(wind_estimate.tas_innov_var); _ekf.getBetaInnov(wind_estimate.beta_innov); _ekf.getBetaInnovVar(wind_estimate.beta_innov_var); - wind_estimate.timestamp = timestamp; + wind_estimate.windspeed_north = wind_vel(0); wind_estimate.windspeed_east = wind_vel(1); wind_estimate.variance_north = wind_vel_var(0); wind_estimate.variance_east = wind_vel_var(1); wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf + wind_estimate.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _wind_pub.publish(wind_estimate); } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index a5a43fa4e6..fb4878e7ab 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -500,9 +500,13 @@ void BlockLocalPositionEstimator::Run() publishLocalPos(); publishOdom(); publishEstimatorStatus(); - _pub_innov.get().timestamp = _timeStamp; + + _pub_innov.get().timestamp_sample = _timeStamp; + _pub_innov.get().timestamp = hrt_absolute_time(); _pub_innov.update(); - _pub_innov_var.get().timestamp = _timeStamp; + + _pub_innov_var.get().timestamp_sample = _timeStamp; + _pub_innov_var.get().timestamp = hrt_absolute_time(); _pub_innov_var.update(); if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) { @@ -578,7 +582,7 @@ void BlockLocalPositionEstimator::publishLocalPos() 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_lpos.get().timestamp = _timeStamp; + _pub_lpos.get().timestamp_sample = _timeStamp; _pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY; _pub_lpos.get().z_valid = _estimatorInitialized & EST_Z; @@ -628,6 +632,7 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().vz_max = INFINITY; _pub_lpos.get().hagl_min = INFINITY; _pub_lpos.get().hagl_max = INFINITY; + _pub_lpos.get().timestamp = hrt_absolute_time();; _pub_lpos.update(); } } @@ -640,7 +645,7 @@ 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 = hrt_absolute_time(); + _pub_odom.get().timestamp_sample = _timeStamp; _pub_odom.get().local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; @@ -706,14 +711,14 @@ void BlockLocalPositionEstimator::publishOdom() _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCHRATE_VARIANCE] = NAN; _pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAWRATE_VARIANCE] = NAN; + _pub_odom.get().timestamp = hrt_absolute_time(); _pub_odom.update(); } } void BlockLocalPositionEstimator::publishEstimatorStatus() { - _pub_est_states.get().timestamp = _timeStamp; - _pub_est_status.get().timestamp = _timeStamp; + _pub_est_states.get().timestamp_sample = _timeStamp; for (size_t i = 0; i < n_x; i++) { _pub_est_states.get().states[i] = _x(i); @@ -752,11 +757,17 @@ void BlockLocalPositionEstimator::publishEstimatorStatus() _pub_est_states.get().covariances[23] = NAN; _pub_est_states.get().n_states = n_x; + _pub_est_states.get().timestamp = hrt_absolute_time(); + _pub_est_states.update(); + + // estimator_status + _pub_est_status.get().timestamp_sample = _timeStamp; _pub_est_status.get().health_flags = _sensorFault; _pub_est_status.get().timeout_flags = _sensorTimeout; _pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph; _pub_est_status.get().pos_vert_accuracy = _pub_gpos.get().epv; + _pub_est_status.get().timestamp = hrt_absolute_time(); _pub_est_status.update(); } @@ -790,7 +801,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) && PX4_ISFINITE(xLP(X_vz))) { - _pub_gpos.get().timestamp = _timeStamp; + _pub_gpos.get().timestamp_sample = _timeStamp; _pub_gpos.get().lat = lat; _pub_gpos.get().lon = lon; _pub_gpos.get().alt = alt; @@ -799,6 +810,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz); _pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ; _pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY); + _pub_gpos.get().timestamp = hrt_absolute_time(); _pub_gpos.update(); } }