Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Replay: added --logmatch option 

match logging rate to input log
This commit is contained in:
Andrew Tridgell 2016-05-05 14:08:52 +10:00
parent 679b43efc9
commit 7abaa75653
4 changed files with 38 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/Replay/LR_MsgHandler.cpp
View File

@ -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);
}

4
Tools/Replay/LR_MsgHandler.h
View File

@ -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) { };

6
Tools/Replay/LogReader.cpp
View File

@ -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,

43
Tools/Replay/Replay.cpp
View File

@ -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();
}
}