diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 77b4569b10..baa71dbbcf 100644 --- a/Tools/Replay/LogReader.cpp +++ b/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); diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 296c969902..f11884cdbc 100644 --- a/Tools/Replay/LogReader.h +++ b/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; diff --git a/Tools/Replay/Parameters.h b/Tools/Replay/Parameters.h new file mode 100644 index 0000000000..3a79b0f17b --- /dev/null +++ b/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 + +// 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 diff --git a/Tools/Replay/Parameters.pde b/Tools/Replay/Parameters.pde new file mode 100644 index 0000000000..23561cabed --- /dev/null +++ b/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")); + } +} diff --git a/Tools/Replay/Replay.pde b/Tools/Replay/Replay.pde index 646ab4a83c..0534c9742b 100644 --- a/Tools/Replay/Replay.pde +++ b/Tools/Replay/Replay.pde @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -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; iupdate(); if (g_gps->status() >= GPS::GPS_OK_FIX_3D) {