/*
find information about the log
*/
bool Replay::find_log_info(struct log_information &info)
{
IMUCounter reader;
if (!reader.open_log(filename)) {
perror(filename);
exit(1);
}
char clock_source[5] = { };
int samplecount = 0;
uint64_t prev = 0;
uint64_t smallest_delta = 0;
prev = 0;
const uint16_t samples_required = 1000;
while (samplecount < samples_required) {
char type[5];
if (!reader.update(type)) {
break;
}
if (strlen(clock_source) == 0) {
// if you want to add a clock source, also add it to
// handle_msg and handle_log_format_msg, above
if (streq(type, "IMU")) {
memcpy(clock_source, "IMU", 3);
} else if (streq(type, "IMT")) {
memcpy(clock_source, "IMT", 3);
} else {
continue;
}
}
if (streq(type, clock_source)) {
if (prev == 0) {
prev = reader.last_clock_timestamp;
} else {
uint64_t delta = reader.last_clock_timestamp - prev;
if (smallest_delta == 0 || delta < smallest_delta) {
smallest_delta = delta;
}
samplecount++;
}
}
if (streq(type, "IMU2") && !info.have_imu2) {
info.have_imu2 = true;
}
}
if (smallest_delta == 0) {
::printf("Unable to determine log rate - insufficient IMU/IMT messages? (need=%d got=%d)", samples_required, samplecount);
return false;
}
float rate = 1.0e6f/smallest_delta;
if (rate < 100) {
info.update_rate = 50;
} else {
info.update_rate = 400;
}
return true;
}
/*
find information about the log
*/
bool Replay::find_log_info(struct log_information &info)
{
IMUCounter reader;
if (!reader.open_log(filename)) {
perror(filename);
exit(1);
}
int samplecount = 0;
uint64_t prev = 0;
uint64_t smallest_delta = 0;
prev = 0;
while (samplecount < 1000) {
char type[5];
if (!reader.update(type)) {
break;
}
if (streq(type, "IMU")) {
if (prev == 0) {
prev = reader.last_imu_timestamp;
} else {
uint64_t delta = reader.last_imu_timestamp - prev;
if (smallest_delta == 0 || delta < smallest_delta) {
smallest_delta = delta;
}
samplecount++;
}
}
if (streq(type, "IMU2") && !info.have_imu2) {
info.have_imu2 = true;
}
}
if (smallest_delta == 0) {
::printf("Unable to determine log rate - insufficient IMU messages?!");
return false;
}
float rate = 1.0e6f/smallest_delta;
if (rate < 100) {
info.update_rate = 50;
} else {
info.update_rate = 400;
}
return true;
}
/*
find information about the log
*/
bool Replay::find_log_info(struct log_information &info)
{
IMUCounter reader;
if (!reader.open_log(filename)) {
perror(filename);
exit(1);
}
char clock_source[5] = { };
int samplecount = 0;
uint64_t prev = 0;
uint64_t smallest_delta = 0;
uint64_t total_delta = 0;
prev = 0;
const uint16_t samples_required = 1000;
while (samplecount < samples_required) {
char type[5];
if (!reader.update(type)) {
break;
}
if (strlen(clock_source) == 0) {
// If you want to add a clock source, also add it to
// handle_msg and handle_log_format_msg, above. Note that
// ordering is important here. For example, when we log
// IMT we may reduce the logging speed of IMU, so then
// using IMU as your clock source will lead to incorrect
// behaviour.
if (streq(type, "IMT")) {
strcpy(clock_source, "IMT");
} else if (streq(type, "IMU")) {
strcpy(clock_source, "IMU");
} else {
continue;
}
hal.console->printf("Using clock source %s\n", clock_source);
}
// IMT if available always overrides
if (streq(type, "IMT") && strcmp(clock_source, "IMT") != 0) {
strcpy(clock_source, "IMT");
hal.console->printf("Changing clock source to %s\n", clock_source);
samplecount = 0;
prev = 0;
smallest_delta = 0;
total_delta = 0;
}
if (streq(type, clock_source)) {
if (prev == 0) {
prev = reader.last_clock_timestamp;
} else {
uint64_t delta = reader.last_clock_timestamp - prev;
if (delta < 40000 && delta > 1000) {
if (smallest_delta == 0 || delta < smallest_delta) {
smallest_delta = delta;
}
samplecount++;
total_delta += delta;
}
}
prev = reader.last_clock_timestamp;
}
if (streq(type, "IMU2")) {
info.have_imu2 = true;
}
if (streq(type, "IMT")) {
info.have_imt = true;
}
if (streq(type, "IMT2")) {
info.have_imt2 = true;
}
}
if (smallest_delta == 0) {
::printf("Unable to determine log rate - insufficient IMU/IMT messages? (need=%d got=%d)", samples_required, samplecount);
return false;
}
float average_delta = total_delta / samplecount;
float rate = 1.0e6f/average_delta;
printf("average_delta=%.2f smallest_delta=%lu samplecount=%lu\n",
average_delta, (unsigned long)smallest_delta, (unsigned long)samplecount);
if (rate < 100) {
info.update_rate = 50;
} else {
info.update_rate = 400;
}
return true;
}
