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