From 4da93a316d9c15b0599c6f16c4e531013289974d Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 1 Apr 2016 13:01:45 +0200 Subject: [PATCH] ekf2 replay logic: support recent change in ekf2 module which made it only publish the output when an ekf update was occuring. in case the ekf2 module does not update the replay module will be informed and will thus not wait for an update but continue preparing the next sample of sensor data --- src/modules/ekf2/ekf2_main.cpp | 13 ++++++++++++- src/modules/ekf2_replay/ekf2_replay_main.cpp | 12 ++++++++++-- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index d704aafd4e..e3182a1167 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -468,7 +468,6 @@ void Ekf2::task_main() // run the EKF update and output if (_ekf->update()) { - // generate vehicle attitude quaternion data struct vehicle_attitude_s att = {}; _ekf->copy_quaternion(att.q); @@ -626,6 +625,18 @@ void Ekf2::task_main() orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); } } + } else if (_replay_mode) { + // in replay mode we have to tell the replay module not to wait for an update + // we do this by publishing an attitude with zero timestamp + struct vehicle_attitude_s att = {}; + att.timestamp = 0; + + if (_att_pub == nullptr) { + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + + } else { + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); + } } // publish estimator status diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index ebe96223ff..98d933c3b6 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -436,6 +436,13 @@ void Ekf2Replay::logIfUpdated() // update attitude struct vehicle_attitude_s att = {}; orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + + // if the timestamp of the attitude is zero, then this means that the ekf did not + // do an update so we can ignore this message and just continue + if (att.timestamp == 0) { + return; + } + memset(&log_message.body.att.q_w, 0, sizeof(log_ATT_s)); log_message.type = LOG_ATT_MSG; @@ -695,8 +702,9 @@ void Ekf2Replay::task_main() read_first_header = true; - if (header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2) { - PX4_WARN("bad log header\n"); + if ((header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2)) { + // we assume that the log file is finished here + PX4_WARN("Done!"); _task_should_exit = true; continue; }