diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 617eee9deb..bd68244511 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -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. diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 3345e716a2..de5a99c3bc 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -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 **¬types); 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 **¬types; - 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); }; diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 67b10174e8..149268ae46 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -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",