Replay: added parameter handling

use parameters from log, and allow override

Tools/Replay/LogReader.cpp

@@ -193,6 +193,32 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length)
     }
 }
 
+bool LogReader::set_parameter(const char *name, float value)
+{
+    enum ap_var_type var_type;
+    AP_Param *vp = AP_Param::find(name, &var_type);
+    if (vp == NULL) {
+        return false;
+    }
+    if (var_type == AP_PARAM_FLOAT) {
+        ((AP_Float *)vp)->set(value);
+        ::printf("Set %s to %f\n", name, value);
+    } else if (var_type == AP_PARAM_INT32) {
+        ((AP_Int32 *)vp)->set(value);
+        ::printf("Set %s to %d\n", name, (int)value);
+    } else if (var_type == AP_PARAM_INT16) {
+        ((AP_Int16 *)vp)->set(value);
+        ::printf("Set %s to %d\n", name, (int)value);
+    } else if (var_type == AP_PARAM_INT8) {
+        ((AP_Int8 *)vp)->set(value);
+        ::printf("Set %s to %d\n", name, (int)value);
+    } else {
+        // we don't support mavlink set on this parameter
+        return false;
+    }            
+    return true;
+}
+
 bool LogReader::update(uint8_t &type)
 {
     uint8_t hdr[3];
@@ -325,6 +351,18 @@ bool LogReader::update(uint8_t &type)
         break;
     }
 
+    case LOG_PARAMETER_MSG: {
+        struct log_Parameter msg;
+        if(sizeof(msg) != f.length) {
+            printf("Bad LOG_PARAMETER length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        set_parameter(msg.name, msg.value);
+        break;        
+    }
+        
+
     default:
         if (vehicle == VEHICLE_PLANE) {
             process_plane(f.type, data, f.length);

Tools/Replay/LogReader.h

@@ -29,6 +29,8 @@ public:
 
     vehicle_type vehicle;
 
+    bool set_parameter(const char *name, float value);
+
 private:
     int fd;
     AP_InertialSensor &ins;

Tools/Replay/Parameters.h

@@ -0,0 +1,25 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef PARAMETERS_H
+#define PARAMETERS_H
+
+#include <AP_Common.h>
+
+// Global parameter class.
+//
+class Parameters {
+public:
+    enum {
+        k_param_dummy,
+        k_param_barometer,
+        k_param_ins,
+        k_param_ahrs,
+        k_param_airspeed,
+        k_param_NavEKF
+    };
+    AP_Int8 dummy;
+};
+
+extern const AP_Param::Info var_info[];
+
+#endif // PARAMETERS_H

Tools/Replay/Parameters.pde

@@ -0,0 +1,45 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+ *  Replay parameter definitions
+ *
+ */
+
+#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} }
+#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} }
+#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
+
+const AP_Param::Info var_info[] PROGMEM = {
+    GSCALAR(dummy,         "_DUMMY", 0),
+
+    // barometer ground calibration. The GND_ prefix is chosen for
+    // compatibility with previous releases of ArduPlane
+    // @Group: GND_
+    // @Path: ../libraries/AP_Baro/AP_Baro.cpp
+    GOBJECT(barometer, "GND_", AP_Baro),
+
+    // @Group: INS_
+    // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
+    GOBJECT(ins,                    "INS_", AP_InertialSensor),
+
+    // @Group: AHRS_
+    // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
+    GOBJECT(ahrs,                   "AHRS_",    AP_AHRS),
+
+    // @Group: ARSPD_
+    // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
+    GOBJECT(airspeed,                               "ARSPD_",   AP_Airspeed),
+
+    // @Group: EKF_
+    // @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp
+    GOBJECTN(ahrs.get_NavEKF(), NavEKF, "EKF_", NavEKF),
+
+    AP_VAREND
+};
+
+static void load_parameters(void)
+{
+    if (!AP_Param::check_var_info()) {
+        hal.scheduler->panic(PSTR("Bad parameter table"));
+    }
+}

Tools/Replay/Replay.pde

@@ -42,6 +42,7 @@
 #include <AP_InertialSensor.h>
 #include <AP_InertialNav.h>
 #include <AP_NavEKF.h>
+#include <Parameters.h>
 #include <stdio.h>
 #include <getopt.h>
 #include <errno.h>
@@ -50,6 +51,10 @@
 
 const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 
+AP_Param param_loader(var_info, 4096);
+
+static Parameters g;
+
 static AP_InertialSensor_HIL ins;
 static AP_Baro_HIL barometer;
 static AP_GPS_HIL gps_driver;
@@ -76,10 +81,25 @@ static FILE *ekf2f;
 static FILE *ekf3f;
 static FILE *ekf4f;
 
+static bool done_parameters;
 static bool done_baro_init;
 static bool done_home_init;
 static uint16_t update_rate;
 
+static uint8_t num_user_parameters;
+static struct {
+    char name[17];
+    float value;
+} user_parameters[100];
+
+
+static void usage(void)
+{
+    ::printf("Options:\n");
+    ::printf(" -rRATE     set IMU rate in Hz\n");
+    ::printf(" -pNAME=VALUE set parameter NAME to VALUE\n");
+}
+
 void setup()
 {
     ::printf("Starting\n");
@@ -91,11 +111,30 @@ void setup()
 
     hal.util->commandline_arguments(argc, argv);
 
-	while ((opt = getopt(argc, argv, "r:")) != -1) {
+	while ((opt = getopt(argc, argv, "r:p:h")) != -1) {
 		switch (opt) {
+        case 'h':
+            usage();
+            exit(0);
         case 'r':
 			update_rate = strtol(optarg, NULL, 0);
             break;
+
+        case 'p':
+            char *eq = strchr(optarg, '=');
+            if (eq == NULL) {
+                ::printf("Usage: -p NAME=VALUE\n");
+                exit(1);
+            }
+            *eq++ = 0;
+            strncpy(user_parameters[num_user_parameters].name, optarg, 16);
+            user_parameters[num_user_parameters].value = atof(eq);
+            num_user_parameters++;
+            if (num_user_parameters >= sizeof(user_parameters)/sizeof(user_parameters[0])) {
+                ::printf("Too many user parameters\n");
+                exit(1);
+            }
+            break;
         }
     }
 
@@ -111,6 +150,8 @@ void setup()
         hal.console->printf("Using an update rate of %u Hz\n", update_rate);
     }
 
+    load_parameters();
+
     if (!LogReader.open_log(filename)) {
         perror(filename);
         exit(1);
@@ -189,8 +230,26 @@ void setup()
     }
 }
 
+
+/*
+  setup user -p parameters
+ */
+static void set_user_parameters(void)
+{
+    for (uint8_t i=0; i<num_user_parameters; i++) {
+        if (!LogReader.set_parameter(user_parameters[i].name, user_parameters[i].value)) {
+            ::printf("Failed to set parameter %s to %f\n", user_parameters[i].name, user_parameters[i].value);
+            exit(1);
+        }
+    }
+}
+
 static void read_sensors(uint8_t type)
 {
+    if (!done_parameters && type != LOG_FORMAT_MSG && type != LOG_PARAMETER_MSG) {
+        done_parameters = true;
+        set_user_parameters();
+    }
     if (type == LOG_GPS_MSG) {
         g_gps->update();
         if (g_gps->status() >= GPS::GPS_OK_FIX_3D) {