yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: use sensors timestamp for published topics when in replay mode

Browse Source 
when doing fast replay, hrt_absolute_time() will not match the replayed time
thus we just use the same timestamp as the input sensors.
This commit is contained in:
Beat Küng
2017-03-01 10:41:08 +01:00
committed by Lorenz Meier
parent 018846fadf
commit 2220c3a60d
1 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/ekf2/ekf2_main.cpp
View File

@@ -702,7 +702,7 @@ void Ekf2::task_main()
control_state_s ctrl_state = {};
float gyro_bias[3] = {};
_ekf.get_gyro_bias(gyro_bias);
ctrl_state.timestamp = hrt_absolute_time();
ctrl_state.timestamp = _replay_mode ? now : hrt_absolute_time();
gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
@@ -784,7 +784,7 @@ void Ekf2::task_main()
{
// generate vehicle attitude quaternion data
struct vehicle_attitude_s att = {};
att.timestamp = hrt_absolute_time();
att.timestamp = _replay_mode ? now : hrt_absolute_time();
q.copyTo(att.q);
@@ -805,7 +805,7 @@ void Ekf2::task_main()
struct vehicle_local_position_s lpos = {};
float pos[3] = {};
lpos.timestamp = hrt_absolute_time();
lpos.timestamp = _replay_mode ? now : hrt_absolute_time();
// Position of body origin in local NED frame
_ekf.get_position(pos);
@@ -868,7 +868,7 @@ void Ekf2::task_main()
// generate and publish global position data
struct vehicle_global_position_s global_pos = {};
global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
global_pos.timestamp = _replay_mode ? now : hrt_absolute_time();
global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
double est_lat, est_lon, lat_pre_reset, lon_pre_reset;
@@ -933,7 +933,7 @@ void Ekf2::task_main()
// publish estimator status
struct estimator_status_s status = {};
status.timestamp = hrt_absolute_time();
status.timestamp = _replay_mode ? now : hrt_absolute_time();
_ekf.get_state_delayed(status.states);
_ekf.get_covariances(status.covariances);
_ekf.get_gps_check_status(&status.gps_check_fail_flags);
@@ -957,7 +957,7 @@ void Ekf2::task_main()
// Publish wind estimate
struct wind_estimate_s wind_estimate = {};
wind_estimate.timestamp = hrt_absolute_time();
wind_estimate.timestamp = _replay_mode ? now : hrt_absolute_time();
wind_estimate.windspeed_north = status.states[22];
wind_estimate.windspeed_east = status.states[23];
wind_estimate.covariance_north = status.covariances[22];
@@ -973,7 +973,7 @@ void Ekf2::task_main()
// publish estimator innovation data
{
struct ekf2_innovations_s innovations = {};
innovations.timestamp = hrt_absolute_time();
innovations.timestamp = _replay_mode ? now : hrt_absolute_time();
_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
_ekf.get_mag_innov(&innovations.mag_innov[0]);
_ekf.get_heading_innov(&innovations.heading_innov);