diff --git a/Tools/Replay/CheckLogs.py b/Tools/Replay/CheckLogs.py index 2c35362f0f..1e6315dd1e 100755 --- a/Tools/Replay/CheckLogs.py +++ b/Tools/Replay/CheckLogs.py @@ -34,7 +34,7 @@ def run_replay(logfile): opts.tolerance_euler, opts.tolerance_pos, opts.tolerance_vel) - run_cmd(cmd, checkfail=True) + run_cmd(cmd, checkfail=False) def get_log_list(): '''get a list of log files to process''' @@ -158,12 +158,18 @@ def check_logs(): def create_checked_logs(): '''create a set of CHEK logs''' import glob, os, sys - pattern = os.path.join(opts.logdir, "*.bin") - full_file_list = glob.glob(pattern) + if os.path.isfile(opts.logdir): + full_file_list = [opts.logdir] + else: + pattern = os.path.join(opts.logdir, "*.bin") + full_file_list = glob.glob(pattern) file_list = [] for f in full_file_list: if not f.endswith("-checked.bin"): file_list.append(f) + if len(file_list) == 0: + print("No files to process") + sys.exit(1) for f in file_list: print("Processing %s" % f) log_list_current = set(glob.glob("logs/*.BIN")) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index d468c51d16..5397d908f6 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -24,7 +24,7 @@ void LR_MsgHandler::wait_timestamp(uint32_t timestamp) void LR_MsgHandler::wait_timestamp_from_msg(uint8_t *msg) { uint64_t time_us; - uint64_t time_ms; + uint32_t time_ms; if (field_value(msg, "TimeUS", time_us)) { // 64-bit timestamp present - great! diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index f5baef347f..a42e42c0fe 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -119,7 +119,8 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype) return mapped_msgid[intype]; } -bool LogReader::handle_log_format_msg(const struct log_Format &f) { +bool LogReader::handle_log_format_msg(const struct log_Format &f) +{ char name[5]; memset(name, '\0', 5); memcpy(name, f.name, 4); @@ -137,6 +138,10 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) { dataflash.WriteBlock(&f_mapped, sizeof(f_mapped)); } + if (msgparser[f.type] != NULL) { + return true; + } + // map from format name to a parser subclass: if (streq(name, "PARM")) { parameter_handler = new LR_MsgHandler_PARM(formats[f.type], dataflash, diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 6b9aa24f5b..9e1ebf4976 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -45,7 +45,7 @@ private: uint32_t ground_alt_cm; - class LR_MsgHandler *msgparser[LOGREADER_MAX_FORMATS]; + class LR_MsgHandler *msgparser[LOGREADER_MAX_FORMATS] {}; Vector3f attitude; Vector3f ahr2_attitude; diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 32274fbe3b..7f542bfa1a 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -206,6 +206,14 @@ public: bool check_solution = false; const char *log_filename = NULL; + /* + information about a log from find_log_info + */ + struct log_information { + uint16_t update_rate; + bool have_imu2; + } log_info {}; + private: const char *filename; ReplayVehicle &_vehicle; @@ -226,10 +234,8 @@ private: bool done_parameters; bool done_baro_init; bool done_home_init; - uint16_t update_rate = 0; int32_t arm_time_ms = -1; bool ahrs_healthy; - bool have_imu2 = false; bool have_imt = false; bool have_imt2 = false; bool have_fram = false; @@ -263,7 +269,9 @@ private: void read_sensors(const char *type); void log_check_generate(); void log_check_solution(); + bool show_error(const char *text, float max_error, float tolerance); void report_checks(); + bool find_log_info(struct log_information &info); }; Replay replay(replayvehicle); @@ -271,7 +279,6 @@ Replay replay(replayvehicle); void Replay::usage(void) { ::printf("Options:\n"); - ::printf("\t--rate RATE set IMU rate in Hz\n"); ::printf("\t--parm NAME=VALUE set parameter NAME to VALUE\n"); ::printf("\t--accel-mask MASK set accel mask (1=accel1 only, 2=accel2 only, 3=both)\n"); ::printf("\t--gyro-mask MASK set gyro mask (1=gyro1 only, 2=gyro2 only, 3=both)\n"); @@ -300,7 +307,6 @@ void Replay::flush_dataflash(void) { void Replay::_parse_command_line(uint8_t argc, char * const argv[]) { const struct GetOptLong::option options[] = { - {"rate", true, 0, 'r'}, {"parm", true, 0, 'p'}, {"param", true, 0, 'p'}, {"help", false, 0, 'h'}, @@ -321,10 +327,6 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[]) int opt; while ((opt = gopt.getoption()) != -1) { switch (opt) { - case 'r': - update_rate = strtol(gopt.optarg, NULL, 0); - break; - case 'g': logreader.set_gyro_mask(strtol(gopt.optarg, NULL, 0)); break; @@ -394,86 +396,87 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[]) } } -class IMU2Counter : public DataFlashFileReader { +class IMUCounter : public DataFlashFileReader { public: - IMU2Counter() {} + IMUCounter() {} bool handle_log_format_msg(const struct log_Format &f); bool handle_msg(const struct log_Format &f, uint8_t *msg); - uint64_t last_imu2_timestamp; + uint64_t last_imu_timestamp; private: MsgHandler *handler; }; -bool IMU2Counter::handle_log_format_msg(const struct log_Format &f) { - if (!strncmp(f.name,"IMU2",4)) { - // an IMU2 message + +bool IMUCounter::handle_log_format_msg(const struct log_Format &f) { + if (!strncmp(f.name,"IMU",4)) { + // an IMU message handler = new MsgHandler(f); } return true; }; -bool IMU2Counter::handle_msg(const struct log_Format &f, uint8_t *msg) { - if (strncmp(f.name,"IMU2",4)) { - // not an IMU2 message + +bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg) { + if (strncmp(f.name,"IMU",4)) { + // not an IMU message return true; } - if (handler->field_value(msg, "TimeUS", last_imu2_timestamp)) { -// ::printf("Found timestamp %ld\n", last_imu2_timestamp); - } else if (handler->field_value(msg, "TimeMS", last_imu2_timestamp)) { -// ::printf("Found millisecond timestamp %ld\n", last_imu2_timestamp); - last_imu2_timestamp *= 1000; + if (handler->field_value(msg, "TimeUS", last_imu_timestamp)) { + } else if (handler->field_value(msg, "TimeMS", last_imu_timestamp)) { + last_imu_timestamp *= 1000; } else { - ::printf("Unable to find timestamp in IMU2 message"); + ::printf("Unable to find timestamp in IMU message"); } return true; } -int find_update_rate(const char *filename) { - IMU2Counter reader; +/* + 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 samplesum = 0; + uint64_t smallest_delta = 0; prev = 0; - while (samplecount < 10) { + while (samplecount < 1000) { char type[5]; if (!reader.update(type)) { break; } - if (streq(type, "IMU2")) { + if (streq(type, "IMU")) { if (prev == 0) { - prev = reader.last_imu2_timestamp; + 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++; - samplesum += reader.last_imu2_timestamp - prev; - prev = reader.last_imu2_timestamp; } } + if (streq(type, "IMU2") && !info.have_imu2) { + info.have_imu2 = true; + } } - if (samplecount < 10) { - ::printf("Unable to determine log rate - insufficient IMU2 messages?!"); - exit(1); + if (smallest_delta == 0) { + ::printf("Unable to determine log rate - insufficient IMU messages?!"); + return false; } - float rate = 1000000/int(samplesum/samplecount); - if (abs(rate - 50) < 5) { - return 50; + float rate = 1.0e6f/smallest_delta; + if (rate < 100) { + info.update_rate = 50; + } else { + info.update_rate = 400; } - if (abs(rate - 100) < 10) { - return 100; - } - if (abs(rate - 200) < 10) { - return 200; - } - if (abs(rate - 400) < 20) { // I have a log which is 10 off... - return 400; - } - ::printf("Unable to determine log rate - %f matches no rate\n", rate); - exit(1); + return true; } // catch floating point exceptions @@ -510,11 +513,12 @@ void Replay::setup() // remember filename for reporting log_filename = filename; - if (update_rate == 0) { - update_rate = find_update_rate(filename); + if (!find_log_info(log_info)) { + printf("Update to get log information\n"); + exit(1); } - hal.console->printf("Using an update rate of %u Hz\n", update_rate); + hal.console->printf("Using an update rate of %u Hz\n", log_info.update_rate); if (!logreader.open_log(filename)) { perror(filename); @@ -522,12 +526,7 @@ void Replay::setup() } _vehicle.setup(); - set_ins_update_rate(update_rate); - - logreader.wait_type("GPS"); - logreader.wait_type("IMU"); - logreader.wait_type("GPS"); - logreader.wait_type("IMU"); + set_ins_update_rate(log_info.update_rate); feenableexcept(FE_INVALID | FE_OVERFLOW); @@ -545,32 +544,10 @@ void Replay::setup() fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n"); fprintf(ekf3f, "timestamp TimeMS IVN IVE IVD IPN IPE IPD IMX IMY IMZ IVT\n"); fprintf(ekf4f, "timestamp TimeMS SV SP SH SMX SMY SMZ SVT OFN EFE FS DS\n"); - - ::printf("Waiting for GPS\n"); - while (!done_home_init) { - char type[5]; - if (!logreader.update(type)) { - break; - } - read_sensors(type); - if (streq(type, "GPS") && - (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && - done_baro_init && !done_home_init) { - const Location &loc = _vehicle.gps.location(); - ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", - loc.lat * 1.0e-7f, - loc.lng * 1.0e-7f, - loc.alt * 0.01f, - hal.scheduler->millis()*0.001f); - _vehicle.ahrs.set_home(loc); - _vehicle.compass.set_initial_location(loc.lat, loc.lng); - done_home_init = true; - } - } } void Replay::set_ins_update_rate(uint16_t _update_rate) { - switch (update_rate) { + switch (_update_rate) { case 50: _vehicle.ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ); break; @@ -609,9 +586,6 @@ void Replay::read_sensors(const char *type) done_parameters = true; set_user_parameters(); } - if (streq(type,"IMU2")) { - have_imu2 = true; - } if (use_imt && streq(type,"IMT")) { have_imt = true; } @@ -619,6 +593,21 @@ void Replay::read_sensors(const char *type) have_imt2 = true; } + if (!done_home_init) { + if (streq(type, "GPS") && + (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && done_baro_init) { + const Location &loc = _vehicle.gps.location(); + ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", + loc.lat * 1.0e-7f, + loc.lng * 1.0e-7f, + loc.alt * 0.01f, + hal.scheduler->millis()*0.001f); + _vehicle.ahrs.set_home(loc); + _vehicle.compass.set_initial_location(loc.lat, loc.lng); + done_home_init = true; + } + } + if (streq(type,"GPS")) { _vehicle.gps.update(); if (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) { @@ -656,7 +645,7 @@ void Replay::read_sensors(const char *type) // special handling of IMU messages as these trigger an ahrs.update() if (!have_fram && !have_imt && - ((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2))) { + ((streq(type,"IMU") && !log_info.have_imu2) || (streq(type, "IMU2") && log_info.have_imu2))) { run_ahrs = true; } @@ -980,12 +969,26 @@ void Replay::loop() } } + flush_dataflash(); + if (check_solution) { report_checks(); } exit(0); } + +bool Replay::show_error(const char *text, float max_error, float tolerance) +{ + bool failed = max_error > tolerance; + printf("%s:\t%.2f %c %.2f\n", + text, + max_error, + failed?'>':'<', + tolerance); + return failed; +} + /* report results of --check */ @@ -1006,26 +1009,11 @@ void Replay::report_checks(void) check_result.max_vel_error); fclose(f); } - if (check_result.max_roll_error > tolerance_euler) { - printf("Roll error %.2f > %.2f\n", check_result.max_roll_error, tolerance_euler); - failed = true; - } - if (check_result.max_pitch_error > tolerance_euler) { - printf("Pitch error %.2f > %.2f\n", check_result.max_pitch_error, tolerance_euler); - failed = true; - } - if (check_result.max_yaw_error > tolerance_euler) { - printf("Yaw error %.2f > %.2f\n", check_result.max_yaw_error, tolerance_euler); - failed = true; - } - if (check_result.max_pos_error > tolerance_pos) { - printf("Position error %.2f > %.2f\n", check_result.max_pos_error, tolerance_pos); - failed = true; - } - if (check_result.max_vel_error > tolerance_vel) { - printf("Velocity error %.2f > %.2f\n", check_result.max_vel_error, tolerance_vel); - failed = true; - } + failed |= show_error("Roll error", check_result.max_roll_error, tolerance_euler); + failed |= show_error("Pitch error", check_result.max_pitch_error, tolerance_euler); + failed |= show_error("Yaw error", check_result.max_yaw_error, tolerance_euler); + failed |= show_error("Position error", check_result.max_pos_error, tolerance_pos); + failed |= show_error("Velocity error", check_result.max_vel_error, tolerance_vel); if (failed) { printf("Checks failed\n"); exit(1);