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

Replay: added --nottypes option

This commit is contained in:
Andrew Tridgell 2015-07-07 15:13:51 +10:00
parent 1d732ddf62
commit dd53f5a7a1
3 changed files with 62 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Tools/Replay/LogReader.cpp
View File

@ -28,7 +28,7 @@
extern const AP_HAL::HAL& hal;
LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const struct LogStructure *_structure, uint8_t _num_types) :
LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const struct LogStructure *_structure, uint8_t _num_types, const char **&_nottypes) :
vehicle(VehicleType::VEHICLE_UNKNOWN),
ahrs(_ahrs),
ins(_ins),
@ -42,7 +42,8 @@ LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Co
accel_mask(7),
gyro_mask(7),
last_timestamp_usec(0),
installed_vehicle_specific_parsers(false)
installed_vehicle_specific_parsers(false),
nottypes(_nottypes)
{
}
@ -90,6 +91,9 @@ static const char *generated_names[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5",
*/
bool LogReader::in_list(const char *type, const char *list[])
{
if (list == NULL) {
return false;
}
for (uint8_t i=0; list[i] != NULL; i++) {
if (strcmp(type, list[i]) == 0) {
return true;
@ -251,7 +255,9 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
exit(1);
}
msg[2] = mapped_msgid[msg[2]];
dataflash.WriteBlock(msg, f.length);
if (!in_list(name, nottypes)) {
dataflash.WriteBlock(msg, f.length);
}
// a MsgHandler would probably have found a timestamp and
// caled stop_clock. This runs IO, clearing dataflash's
// buffer.

8
Tools/Replay/LogReader.h
View File

@ -5,7 +5,7 @@
class LogReader : public DataFlashFileReader
{
public:
LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const struct LogStructure *structure, uint8_t num_types);
LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const struct LogStructure *structure, uint8_t num_types, const char **&nottypes);
bool wait_type(const char *type);
const Vector3f &get_attitude(void) const { return attitude; }
@ -27,6 +27,8 @@ public:
virtual bool handle_log_format_msg(const struct log_Format &f);
virtual bool handle_msg(const struct log_Format &f, uint8_t *msg);
static bool in_list(const char *type, const char *list[]);
protected:
virtual void end_format_msgs(void) override;
@ -66,9 +68,9 @@ private:
LR_MsgHandler::CheckState check_state;
bool installed_vehicle_specific_parsers;
void maybe_install_vehicle_specific_parsers();
const char **&nottypes;
bool in_list(const char *type, const char *list[]);
void maybe_install_vehicle_specific_parsers();
uint8_t map_fmt_type(const char *name, uint8_t intype);
};

53
Tools/Replay/Replay.cpp
View File

@ -225,7 +225,7 @@ private:
SITL sitl;
#endif
LogReader logreader{_vehicle.ahrs, _vehicle.ins, _vehicle.barometer, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.dataflash, log_structure, ARRAY_SIZE(log_structure)};
LogReader logreader{_vehicle.ahrs, _vehicle.ins, _vehicle.barometer, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.dataflash, log_structure, ARRAY_SIZE(log_structure), nottypes};
FILE *plotf;
FILE *plotf2;
@ -247,6 +247,7 @@ private:
float tolerance_euler = 3;
float tolerance_pos = 2;
float tolerance_vel = 2;
const char **nottypes = NULL;
struct {
float max_roll_error;
@ -275,6 +276,7 @@ private:
bool show_error(const char *text, float max_error, float tolerance);
void report_checks();
bool find_log_info(struct log_information &info);
const char **parse_list_from_string(const char *str);
};
Replay replay(replayvehicle);
@ -292,6 +294,7 @@ void Replay::usage(void)
::printf("\t--tolerance-euler tolerance for euler angles in degrees\n");
::printf("\t--tolerance-pos tolerance for position in meters\n");
::printf("\t--tolerance-vel tolerance for velocity in meters/second\n");
::printf("\t--nottypes list of msg types not to output, comma separated\n");
}
@ -300,13 +303,42 @@ enum {
OPT_CHECK_GENERATE,
OPT_TOLERANCE_EULER,
OPT_TOLERANCE_POS,
OPT_TOLERANCE_VEL
OPT_TOLERANCE_VEL,
OPT_NOTTYPES
};
void Replay::flush_dataflash(void) {
_vehicle.dataflash.flush();
}
/*
create a list from a comma separated string
*/
const char **Replay::parse_list_from_string(const char *str_in)
{
uint16_t comma_count=0;
const char *p;
for (p=str_in; *p; p++) {
if (*p == ',') comma_count++;
}
char *str = strdup(str_in);
if (str == NULL) {
return NULL;
}
const char **ret = (const char **)calloc(comma_count+2, sizeof(char *));
if (ret == NULL) {
free(str);
return NULL;
}
char *saveptr = NULL;
uint16_t idx = 0;
for (p=strtok_r(str, ",", &saveptr); p; p=strtok_r(NULL, ",", &saveptr)) {
ret[idx++] = p;
}
return ret;
}
void Replay::_parse_command_line(uint8_t argc, char * const argv[])
{
const struct GetOptLong::option options[] = {
@ -322,6 +354,7 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
{"tolerance-euler", true, 0, OPT_TOLERANCE_EULER},
{"tolerance-pos", true, 0, OPT_TOLERANCE_POS},
{"tolerance-vel", true, 0, OPT_TOLERANCE_VEL},
{"nottypes", true, 0, OPT_NOTTYPES},
{0, false, 0, 0}
};
@ -384,6 +417,10 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
tolerance_vel = atof(gopt.optarg);
break;
case OPT_NOTTYPES:
nottypes = parse_list_from_string(gopt.optarg);
break;
case 'h':
default:
usage();
@ -664,9 +701,15 @@ void Replay::read_sensors(const char *type)
if (_vehicle.ahrs.get_home().lat != 0) {
_vehicle.inertial_nav.update(_vehicle.ins.get_delta_time());
}
_vehicle.dataflash.Log_Write_EKF(_vehicle.ahrs,false);
_vehicle.dataflash.Log_Write_AHRS2(_vehicle.ahrs);
_vehicle.dataflash.Log_Write_POS(_vehicle.ahrs);
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 (_vehicle.ahrs.healthy() != ahrs_healthy) {
ahrs_healthy = _vehicle.ahrs.healthy();
printf("AHRS health: %u at %lu\n",