diff --git a/msg/estimator_innovations.msg b/msg/estimator_innovations.msg index 311e840e54..cb3e85a1c4 100644 --- a/msg/estimator_innovations.msg +++ b/msg/estimator_innovations.msg @@ -13,12 +13,6 @@ float32 ev_vvel # vertical external vision velocity innovation (m/sec) and in float32[2] ev_hpos # horizontal external vision position innovation (m) and innovation variance (m**2) float32 ev_vpos # vertical external vision position innovation (m) and innovation variance (m**2) -# Fake Position and Velocity -float32[2] fake_hvel # fake horizontal velocity innovation (m/s) and innovation variance ((m/s)**2) -float32 fake_vvel # fake vertical velocity innovation (m/s) and innovation variance ((m/s)**2) -float32[2] fake_hpos # fake horizontal position innovation (m) and innovation variance (m**2) -float32 fake_vpos # fake vertical position innovation (m) and innovation variance (m**2) - # Height sensors float32 rng_vpos # range sensor height innovation (m) and innovation variance (m**2) float32 baro_vpos # barometer height innovation (m) and innovation variance (m**2) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 2e8f66358e..ac97af886c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -582,8 +582,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp, const imuSample &imu _ekf.getHaglInnov(innovations.hagl); // Not yet supported innovations.aux_vvel = NAN; - innovations.fake_hpos[0] = innovations.fake_hpos[1] = innovations.fake_vpos = NAN; - innovations.fake_hvel[0] = innovations.fake_hvel[1] = innovations.fake_vvel = NAN; + innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovations_pub.publish(innovations); @@ -622,8 +621,6 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getHaglInnovRatio(test_ratios.hagl); // Not yet supported test_ratios.aux_vvel = NAN; - test_ratios.fake_hpos[0] = test_ratios.fake_hpos[1] = test_ratios.fake_vpos = NAN; - test_ratios.fake_hvel[0] = test_ratios.fake_hvel[1] = test_ratios.fake_vvel = NAN; test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovation_test_ratios_pub.publish(test_ratios); @@ -648,8 +645,6 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) _ekf.getHaglInnovVar(variances.hagl); // Not yet supported variances.aux_vvel = NAN; - variances.fake_hpos[0] = variances.fake_hpos[1] = variances.fake_vpos = NAN; - variances.fake_hvel[0] = variances.fake_hvel[1] = variances.fake_vvel = NAN; variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovation_variances_pub.publish(variances);