diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 07b349c824..dcec234918 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -71,7 +71,7 @@ void LR_MsgHandler_ARSP::process_message(uint8_t *msg) require_field_float(msg, "Temp")); } -void LR_MsgHandler_FRAM::process_message(uint8_t *msg) +void LR_MsgHandler_NKF1::process_message(uint8_t *msg) { wait_timestamp_from_msg(msg); } diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index 87a8dd2cb4..b192b14f7d 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -69,10 +69,10 @@ private: AP_Airspeed &airspeed; }; -class LR_MsgHandler_FRAM : public LR_MsgHandler +class LR_MsgHandler_NKF1 : public LR_MsgHandler { public: - LR_MsgHandler_FRAM(log_Format &_f, DataFlash_Class &_dataflash, + LR_MsgHandler_NKF1(log_Format &_f, DataFlash_Class &_dataflash, uint64_t &_last_timestamp_usec) : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) { }; diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 2df5e1c633..4aa63c4961 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -255,9 +255,9 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) msgparser[f.type] = new LR_MsgHandler_ARSP(formats[f.type], dataflash, last_timestamp_usec, airspeed); - } else if (streq(name, "FRAM")) { - msgparser[f.type] = new LR_MsgHandler_FRAM(formats[f.type], dataflash, - last_timestamp_usec); + } else if (streq(name, "NKF1")) { + msgparser[f.type] = new LR_MsgHandler_NKF1(formats[f.type], dataflash, + last_timestamp_usec); } else if (streq(name, "CHEK")) { msgparser[f.type] = new LR_MsgHandler_CHEK(formats[f.type], dataflash, last_timestamp_usec, diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 772e4b3a5b..5d342cd09b 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -263,6 +263,7 @@ private: float tolerance_vel = 2; const char **nottypes = NULL; uint16_t downsample = 0; + bool logmatch = false; uint32_t output_counter = 0; struct { @@ -288,6 +289,7 @@ private: void usage(void); void set_user_parameters(void); void read_sensors(const char *type); + void write_ekf_logs(void); void log_check_generate(); void log_check_solution(); bool show_error(const char *text, float max_error, float tolerance); @@ -313,6 +315,7 @@ void Replay::usage(void) ::printf("\t--tolerance-vel tolerance for velocity in meters/second\n"); ::printf("\t--nottypes list of msg types not to output, comma separated\n"); ::printf("\t--downsample downsampling rate for output\n"); + ::printf("\t--logmatch match logging rate to source\n"); } @@ -323,7 +326,8 @@ enum { OPT_TOLERANCE_POS, OPT_TOLERANCE_VEL, OPT_NOTTYPES, - OPT_DOWNSAMPLE + OPT_DOWNSAMPLE, + OPT_LOGMATCH }; void Replay::flush_dataflash(void) { @@ -375,6 +379,7 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[]) {"tolerance-vel", true, 0, OPT_TOLERANCE_VEL}, {"nottypes", true, 0, OPT_NOTTYPES}, {"downsample", true, 0, OPT_DOWNSAMPLE}, + {"logmatch", false, 0, OPT_LOGMATCH}, {0, false, 0, 0} }; @@ -445,6 +450,10 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[]) downsample = atoi(gopt.optarg); break; + case OPT_LOGMATCH: + logmatch = true; + break; + case 'h': default: usage(); @@ -694,6 +703,22 @@ void Replay::set_user_parameters(void) } } +/* + write out EKF log messages + */ +void Replay::write_ekf_logs(void) +{ + if (!LogReader::in_list("EKF", nottypes)) { + _vehicle.dataflash.Log_Write_EKF(_vehicle.ahrs,false); + } + if (!LogReader::in_list("AHRS2", nottypes)) { + _vehicle.dataflash.Log_Write_AHRS2(_vehicle.ahrs); + } + if (!LogReader::in_list("POS", nottypes)) { + _vehicle.dataflash.Log_Write_POS(_vehicle.ahrs); + } +} + void Replay::read_sensors(const char *type) { if (!done_parameters && !streq(type,"FMT") && !streq(type,"PARM")) { @@ -763,16 +788,8 @@ void Replay::read_sensors(const char *type) if (_vehicle.ahrs.get_home().lat != 0) { _vehicle.inertial_nav.update(_vehicle.ins.get_delta_time()); } - if (downsample == 0 || ++output_counter % downsample == 0) { - if (!LogReader::in_list("EKF", nottypes)) { - _vehicle.dataflash.Log_Write_EKF(_vehicle.ahrs,false); - } - if (!LogReader::in_list("AHRS2", nottypes)) { - _vehicle.dataflash.Log_Write_AHRS2(_vehicle.ahrs); - } - if (!LogReader::in_list("POS", nottypes)) { - _vehicle.dataflash.Log_Write_POS(_vehicle.ahrs); - } + if ((downsample == 0 || ++output_counter % downsample == 0) && !logmatch) { + write_ekf_logs(); } if (_vehicle.ahrs.healthy() != ahrs_healthy) { ahrs_healthy = _vehicle.ahrs.healthy(); @@ -786,6 +803,10 @@ void Replay::read_sensors(const char *type) log_check_solution(); } } + + if (logmatch && streq(type, "NKF1")) { + write_ekf_logs(); + } }