diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 8185700b80..315302935c 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -1,6 +1,7 @@ #include "LogReader.h" #include "MsgHandler.h" +#include "Replay.h" #include #include @@ -16,10 +17,6 @@ #define streq(x, y) (!strcmp(x, y)) -// const struct LogStructure running_codes_log_structure[] = { -// LOG_COMMON_STRUCTURES, -// }; - LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3) : AP_LoggerFileReader(), _log_structure(log_structure), @@ -254,14 +251,31 @@ bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) { return true; } - #include - #include -bool LogReader::set_parameter(const char *name, float value) +#include +#include + +extern struct user_parameter *user_parameters; + +/* + see if a user parameter is set + */ +bool LogReader::check_user_param(const char *name) { - // if (!strcmp(name, "EK3_ENABLE")) { - // kill(0, SIGTRAP); - // } + for (struct user_parameter *u=user_parameters; u; u=u->next) { + if (strcmp(name, u->name) == 0) { + return true; + } + } + return false; +} + +bool LogReader::set_parameter(const char *name, float value, bool force) +{ + if (!force && check_user_param(name)) { + // ignore user set parameters + return false; + } enum ap_var_type var_type; AP_Param *vp = AP_Param::find(name, &var_type); if (vp == NULL) { diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 879dfe355e..fcfa2b783e 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -19,7 +19,8 @@ public: VehicleType::vehicle_type vehicle; - bool set_parameter(const char *name, float value); + bool check_user_param(const char *name); + bool set_parameter(const char *name, float value, bool force=false); void set_accel_mask(uint8_t mask) { accel_mask = mask; } void set_gyro_mask(uint8_t mask) { gyro_mask = mask; } diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index df76380122..bea3f7fa19 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -30,6 +30,9 @@ static ReplayVehicle replayvehicle; +// list of user parameters +user_parameter *user_parameters; + #define GSCALAR(v, name, def) { replayvehicle.g.v.vtype, name, Parameters::k_param_ ## v, &replayvehicle.g.v, {def_value : def} } #define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &replayvehicle.v, {group_info : class::var_info} } #define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &replayvehicle.v, {group_info : class::var_info} } @@ -116,20 +119,56 @@ void ReplayVehicle::init_ardupilot(void) logger.set_force_log_disarmed(true); } +void Replay::usage(void) +{ + ::printf("Options:\n"); + ::printf("\t--parm NAME=VALUE set parameter NAME to VALUE\n"); + ::printf("\t--param-file FILENAME load parameters from a file\n"); +} + void Replay::_parse_command_line(uint8_t argc, char * const argv[]) { const struct GetOptLong::option options[] = { // name has_arg flag val + {"parm", true, 0, 'p'}, + {"param", true, 0, 'p'}, + {"param-file", true, 0, 'F'}, + {"help", false, 0, 'h'}, {0, false, 0, 0} }; - GetOptLong gopt(argc, argv, "r:p:ha:g:A:n", options); + GetOptLong gopt(argc, argv, "p:F:h", options); int opt; while ((opt = gopt.getoption()) != -1) { + switch (opt) { + case 'p': { + const char *eq = strchr(gopt.optarg, '='); + if (eq == NULL) { + ::printf("Usage: -p NAME=VALUE\n"); + exit(1); + } + struct user_parameter *u = new user_parameter; + strncpy(u->name, gopt.optarg, eq-gopt.optarg); + u->value = atof(eq+1); + u->next = user_parameters; + user_parameters = u; + break; + } + + case 'F': + load_param_file(gopt.optarg); + break; + + case 'h': + default: + usage(); + exit(0); + } } - argv += gopt.optind; - argc -= gopt.optind; + + argv += gopt.optind; + argc -= gopt.optind; if (argc > 0) { filename = argv[0]; @@ -148,6 +187,8 @@ void Replay::setup() _parse_command_line(argc, argv); _vehicle.setup(); + + set_user_parameters(); } void Replay::loop() @@ -167,6 +208,72 @@ void Replay::loop() exit(0); } +/* + setup user -p parameters + */ +void Replay::set_user_parameters(void) +{ + for (struct user_parameter *u=user_parameters; u; u=u->next) { + if (!reader.set_parameter(u->name, u->value, true)) { + ::printf("Failed to set parameter %s to %f\n", u->name, u->value); + exit(1); + } + } +} + +/* + parse a parameter file line + */ +bool Replay::parse_param_line(char *line, char **vname, float &value) +{ + if (line[0] == '#') { + return false; + } + char *saveptr = NULL; + char *pname = strtok_r(line, ", =\t", &saveptr); + if (pname == NULL) { + return false; + } + if (strlen(pname) > AP_MAX_NAME_SIZE) { + return false; + } + const char *value_s = strtok_r(NULL, ", =\t", &saveptr); + if (value_s == NULL) { + return false; + } + value = atof(value_s); + *vname = pname; + return true; +} + + +/* + load a default set of parameters from a file + */ +void Replay::load_param_file(const char *pfilename) +{ + FILE *f = fopen(pfilename, "r"); + if (f == NULL) { + printf("Failed to open parameter file: %s\n", pfilename); + exit(1); + } + char line[100]; + + while (fgets(line, sizeof(line)-1, f)) { + char *pname; + float value; + if (!parse_param_line(line, &pname, value)) { + continue; + } + struct user_parameter *u = new user_parameter; + strncpy(u->name, pname, sizeof(u->name)); + u->value = value; + u->next = user_parameters; + user_parameters = u; + } + fclose(f); +} + Replay replay(replayvehicle); AP_Vehicle& vehicle = replayvehicle; diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h index 9732d03fec..aed5e1a354 100644 --- a/Tools/Replay/Replay.h +++ b/Tools/Replay/Replay.h @@ -17,6 +17,14 @@ #include "LogReader.h" +struct user_parameter { + struct user_parameter *next; + char name[17]; + float value; +}; + +extern user_parameter *user_parameters; + class ReplayVehicle : public AP_Vehicle { public: friend class Replay; @@ -67,6 +75,9 @@ public: void setup() override; void loop() override; + // return true if a user parameter of name is set + bool check_user_param(const char *name); + private: const char *filename; ReplayVehicle &_vehicle; @@ -75,5 +86,8 @@ private: void _parse_command_line(uint8_t argc, char * const argv[]); - + void set_user_parameters(void); + bool parse_param_line(char *line, char **vname, float &value); + void load_param_file(const char *filename); + void usage(); };