From b1f656c4b8badc12e876ca0cbdad477ac403d4b9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 9 Mar 2016 17:08:24 +1100 Subject: [PATCH] ekf2: Add optical flow and range finder data to ekf2 replay logging --- src/modules/ekf2/ekf2_main.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 81e5d98dcb..2f8b25aa58 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -683,6 +683,17 @@ void Ekf2::task_main() replay.vel_d_m_s = gps.vel_d_m_s; replay.vel_ned_valid = gps.vel_ned_valid; + replay.flow_timestamp = optical_flow.timestamp; + replay.range_to_ground = range_finder.current_distance; + + replay.rng_timestamp = range_finder.timestamp; + replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral; + replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral; + replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral; + replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral; + replay.flow_time_integral = optical_flow.integration_timespan; + replay.flow_quality = optical_flow.quality; + if (_replay_pub == nullptr) { _replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);