Replay: support overriding parameters

Tools/Replay/LogReader.cpp

@@ -1,6 +1,7 @@
 #include "LogReader.h"
 
 #include "MsgHandler.h"
+#include "Replay.h"
 
 #include <stdio.h>
 #include <unistd.h>
@@ -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 <sys/types.h>
-       #include <signal.h>
 
-bool LogReader::set_parameter(const char *name, float value)
+#include <sys/types.h>
+#include <signal.h>
+
+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) {

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

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;
 

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