diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6c762bae1a..e6d8ba0082 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1405,17 +1405,17 @@ int sdlog2_thread_main(int argc, char *argv[]) continue; } + // copy topic always + if (record_replay_log) { + orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); + } else { + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); + } if ((poll_counter + 1) % poll_to_logging_factor == 0) { - // the topic will be copied below poll_counter = 0; } else { - // copy topic here in order to reset the updated flag - if (record_replay_log) { - orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); - } else { - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - } + // copy topic poll_counter++; continue; } @@ -1470,7 +1470,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- EKF2 REPLAY --- */ if(record_replay_log) { - orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); + // we poll on the replay topic so we know that it was updated log_msg.msg_type = LOG_RPL1_MSG; log_msg.body.log_RPL1.time_ref = buf.replay.time_ref; log_msg.body.log_RPL1.gyro_integral_dt = buf.replay.gyro_integral_dt; @@ -1531,7 +1531,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } else { /* !record_replay_log */ - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); + /* we poll on sensor combined, so we know it has updated just now */ for (unsigned i = 0; i < 3; i++) { bool write_IMU = false; bool write_SENS = false;