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; }