|
|
|
@ -1405,17 +1405,17 @@ int sdlog2_thread_main(int argc, char *argv[])
@@ -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[])
@@ -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[])
@@ -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; |
|
|
|
|