Browse Source

ekf2: Add optical flow and range finder data to ekf2 replay logging

sbg
Paul Riseborough 9 years ago committed by Lorenz Meier
parent
commit
b1f656c4b8
  1. 11
      src/modules/ekf2/ekf2_main.cpp

11
src/modules/ekf2/ekf2_main.cpp

@ -683,6 +683,17 @@ void Ekf2::task_main() @@ -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);

Loading…
Cancel
Save